Fawkes API Fawkes Development Version
map_lasergen_thread.cpp
1/***************************************************************************
2 * map_lasergen_thread.cpp - Thread to generate laser data from map
3 *
4 * Created: Thu Aug 23 18:43:39 2012
5 * Copyright 2012-2014 Tim Niemueller [www.niemueller.de]
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#include "map_lasergen_thread.h"
22
23#include "amcl_utils.h"
24
25#include <baseapp/run.h>
26#include <utils/math/angle.h>
27
28using namespace fawkes;
29
30/** @class MapLaserGenThread "map_lasergen_thread.h"
31 * Generate laser data from map and position.
32 * @author Tim Niemueller
33 */
34
35/** Constructor. */
37: Thread("MapLaserGenThread", Thread::OPMODE_WAITFORWAKEUP),
38 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
39 TransformAspect(TransformAspect::BOTH_DEFER_PUBLISHER, "Map Laser Odometry")
40{
41 map_ = NULL;
42}
43
44/** Destructor. */
46{
47}
48
49void
51{
52 laser_pose_set_ = false;
53
54 bool have_custom_map = false;
55 try {
56 config->get_string(AMCL_CFG_PREFIX "map-lasergen/map_file");
57 have_custom_map = true;
58 } catch (Exception &e) {
59 } // ignored, use AMCL map
60
61 logger->log_info(name(), "Using %s map configuration", have_custom_map ? "custom" : "AMCL");
62 fawkes::amcl::read_map_config(config,
63 cfg_map_file_,
64 cfg_resolution_,
65 cfg_origin_x_,
66 cfg_origin_y_,
67 cfg_origin_theta_,
68 cfg_occupied_thresh_,
69 cfg_free_thresh_,
70 have_custom_map ? AMCL_CFG_PREFIX "map-lasergen/"
71 : AMCL_CFG_PREFIX);
72
73 cfg_use_current_pose_ = false;
74 try {
75 cfg_use_current_pose_ = config->get_bool(AMCL_CFG_PREFIX "map-lasergen/use_current_pos");
76 } catch (Exception &e) {
77 } // ignored, use default
78
79 cfg_laser_ifname_ = config->get_string(AMCL_CFG_PREFIX "map-lasergen/laser_interface_id");
80 odom_frame_id_ = config->get_string("/frames/odom");
81 base_frame_id_ = config->get_string("/frames/base");
82 laser_frame_id_ = config->get_string(AMCL_CFG_PREFIX "map-lasergen/laser_frame_id");
83
84 if (cfg_use_current_pose_) {
85 cfg_pose_ifname_ = config->get_string(AMCL_CFG_PREFIX "map-lasergen/pose_interface_id");
86 cur_pose_if_ = blackboard->open_for_reading<Position3DInterface>(cfg_pose_ifname_.c_str());
87 } else {
88 pos_x_ = config->get_float(AMCL_CFG_PREFIX "map-lasergen/pos_x");
89 pos_y_ = config->get_float(AMCL_CFG_PREFIX "map-lasergen/pos_y");
90 pos_theta_ = config->get_float(AMCL_CFG_PREFIX "map-lasergen/pos_theta");
91 }
92
93 std::vector<std::pair<int, int>> free_space_indices;
94 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
95 cfg_origin_x_,
96 cfg_origin_y_,
97 cfg_resolution_,
98 cfg_occupied_thresh_,
99 cfg_free_thresh_,
100 free_space_indices);
101
102 map_width_ = map_->size_x;
103 map_height_ = map_->size_y;
104
106 "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
107 map_width_,
108 map_height_,
109 free_space_indices.size(),
110 map_width_ * map_height_,
111 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
112
113 laser_if_ = blackboard->open_for_writing<Laser360Interface>(cfg_laser_ifname_.c_str());
114 gt_pose_if_ = blackboard->open_for_writing<Position3DInterface>("Map LaserGen Groundtruth");
115
116 cfg_add_noise_ = false;
117 try {
118 cfg_add_noise_ = config->get_bool(AMCL_CFG_PREFIX "map-lasergen/add_gaussian_noise");
119 } catch (Exception &e) {
120 }; // ignored
121 if (cfg_add_noise_) {
122#ifndef HAVE_RANDOM
123 throw Exception("Noise has been enabled but C++11 <random> no available at compile time");
124#else
125 cfg_noise_sigma_ = config->get_float(AMCL_CFG_PREFIX "map-lasergen/noise_sigma");
126 std::random_device rd;
127 noise_rg_ = std::mt19937(rd());
128 noise_nd_ = std::normal_distribution<float>(0.0, cfg_noise_sigma_);
129#endif
130 }
131
132 cfg_send_zero_odom_ = false;
133 try {
134 cfg_send_zero_odom_ = config->get_bool(AMCL_CFG_PREFIX "map-lasergen/send_zero_odom");
135 } catch (Exception &e) {
136 }; // ignored
137 if (cfg_send_zero_odom_) {
138 logger->log_info(name(), "Enabling zero odometry publishing");
140 }
141
142 laser_if_->set_frame(laser_frame_id_.c_str());
143}
144
145void
147{
148 if (!laser_pose_set_) {
149 if (set_laser_pose()) {
150 laser_pose_set_ = true;
151
152 if (!cfg_use_current_pose_) {
153 // only need to set it once, let's do it here
154 laser_pos_x_ = pos_x_ + laser_pose_.getOrigin().x();
155 laser_pos_y_ = pos_y_ + laser_pose_.getOrigin().y();
156 laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
157 }
158 } else {
159 if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
160 logger->log_warn(name(), "Could not determine laser pose, skipping loop");
161 }
162 return;
163 }
164 }
165
166 if (cfg_use_current_pose_) {
167 cur_pose_if_->read();
168 pos_x_ = cur_pose_if_->translation(0);
169 pos_y_ = cur_pose_if_->translation(1);
170 pos_theta_ = tf::get_yaw(tf::Quaternion(cur_pose_if_->rotation(0),
171 cur_pose_if_->rotation(1),
172 cur_pose_if_->rotation(2),
173 cur_pose_if_->rotation(3)));
174
175 //logger->log_info(name(), "Pos: (%f,%f,%f)", pos_x_, pos_y_, pos_theta_);
176
177 const float lx = laser_pose_.getOrigin().x();
178 const float ly = laser_pose_.getOrigin().y();
179 laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
180 const float sin_theta = sinf(laser_pos_theta_);
181 const float cos_theta = cosf(laser_pos_theta_);
182 laser_pos_x_ = pos_x_ + (lx * cos_theta - ly * sin_theta);
183 laser_pos_y_ = pos_y_ + (lx * sin_theta + ly * cos_theta);
184 }
185
186 tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
187 gt_pose_if_->set_translation(0, pos_x_);
188 gt_pose_if_->set_translation(1, pos_y_);
189 gt_pose_if_->set_rotation(0, q.x());
190 gt_pose_if_->set_rotation(1, q.y());
191 gt_pose_if_->set_rotation(2, q.z());
192 gt_pose_if_->set_rotation(3, q.w());
193 gt_pose_if_->write();
194
195 float dists[360];
196 for (unsigned int i = 0; i < 360; ++i) {
197 dists[i] = map_calc_range(
198 map_, laser_pos_x_, laser_pos_y_, normalize_rad(deg2rad(i) + laser_pos_theta_), 100.);
199 }
200#ifdef HAVE_RANDOM
201 if (cfg_add_noise_) {
202 for (unsigned int i = 0; i < 360; ++i) {
203 dists[i] += noise_nd_(noise_rg_);
204 }
205 }
206#endif
207 laser_if_->set_distances(dists);
208 laser_if_->write();
209
210 if (cfg_send_zero_odom_) {
211 tf::Transform tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0, 0, 0));
212 Time transform_expiration = (Time(clock) + 1.0);
213
214 tf::StampedTransform tmp_tf_stamped(tmp_tf,
215 transform_expiration,
216 odom_frame_id_,
217 base_frame_id_);
218 tf_publisher->send_transform(tmp_tf_stamped);
219 }
220}
221
222void
224{
225 if (map_) {
226 map_free(map_);
227 map_ = NULL;
228 }
229
230 blackboard->close(laser_if_);
231 blackboard->close(gt_pose_if_);
232 blackboard->close(cur_pose_if_);
233}
234
235bool
236MapLaserGenThread::set_laser_pose()
237{
238 fawkes::Time now(clock);
239 tf::Stamped<tf::Pose> ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
240 &now,
241 laser_frame_id_);
242 try {
243 tf_listener->transform_pose(base_frame_id_, ident, laser_pose_);
244 } catch (fawkes::Exception &e) {
245 return false;
246 }
247
249 "LaserTF: (%f,%f,%f)",
250 laser_pose_.getOrigin().x(),
251 laser_pose_.getOrigin().y(),
252 tf::get_yaw(laser_pose_.getRotation()));
253
254 return true;
255}
virtual ~MapLaserGenThread()
Destructor.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
MapLaserGenThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Position3DInterface Fawkes BlackBoard Interface.
double * rotation() const
Get rotation value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
double * translation() const
Get translation value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
float get_cache_time() const
Get cache time.
void transform_pose(const std::string &target_frame, const Stamped< Pose > &stamped_in, Stamped< Pose > &stamped_out) const
Transform a stamped pose into the target frame.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90