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 
28 using 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 
49 void
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 
105  logger->log_info(name(),
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 
145 void
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 
222 void
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 
235 bool
236 MapLaserGenThread::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 
248  logger->log_debug(name(),
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 }
Laser360Interface Fawkes BlackBoard Interface.
double * rotation() const
Get rotation value.
tf::TransformPublisher * tf_publisher
This is the transform publisher which can be used to publish transforms via the blackboard.
Definition: tf.h:68
virtual void init()
Initialize the thread.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
A class for handling time.
Definition: time.h:92
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
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.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread aspect to access the transform system.
Definition: tf.h:38
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Thread aspect to use blocked timing.
float get_cache_time() const
Get cache time.
MapLaserGenThread()
Constructor.
Position3DInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
Definition: exception.h:35
virtual void send_transform(const StampedTransform &transform, const bool is_static=false)
Publish transform.
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void loop()
Code to execute in the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
double * translation() const
Get translation value.
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:129
virtual ~MapLaserGenThread()
Destructor.
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:145
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void finalize()
Finalize the thread.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.