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 #include "amcl_utils.h"
23 
24 #include <utils/math/angle.h>
25 #include <baseapp/run.h>
26 
27 using namespace fawkes;
28 
29 /** @class MapLaserGenThread "map_lasergen_thread.h"
30  * Generate laser data from map and position.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor. */
36  : Thread("MapLaserGenThread", Thread::OPMODE_WAITFORWAKEUP),
37  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
38  TransformAspect(TransformAspect::BOTH_DEFER_PUBLISHER, "Map Laser Odometry")
39 {
40  map_ = NULL;
41 }
42 
43 /** Destructor. */
45 {
46 }
47 
49 {
50  laser_pose_set_ = false;
51 
52  bool have_custom_map = false;
53  try {
54  config->get_string(AMCL_CFG_PREFIX"map-lasergen/map_file");
55  have_custom_map = true;
56  } catch (Exception &e) {} // ignored, use AMCL map
57 
58  logger->log_info(name(), "Using %s map configuration",
59  have_custom_map ? "custom" : "AMCL");
60  fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
61  cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
62  cfg_free_thresh_,
63  have_custom_map ? AMCL_CFG_PREFIX"map-lasergen/" : AMCL_CFG_PREFIX);
64 
65  cfg_use_current_pose_ = false;
66  try {
67  cfg_use_current_pose_ = config->get_bool(AMCL_CFG_PREFIX"map-lasergen/use_current_pos");
68  } catch (Exception &e) {} // ignored, use default
69 
70  cfg_laser_ifname_ = config->get_string(AMCL_CFG_PREFIX"map-lasergen/laser_interface_id");
71  odom_frame_id_ = config->get_string("/frames/odom");
72  base_frame_id_ = config->get_string("/frames/base");
73  laser_frame_id_ = config->get_string(AMCL_CFG_PREFIX"map-lasergen/laser_frame_id");
74 
75  if (cfg_use_current_pose_) {
76  cfg_pose_ifname_ = config->get_string(AMCL_CFG_PREFIX"map-lasergen/pose_interface_id");
77  cur_pose_if_ = blackboard->open_for_reading<Position3DInterface>(cfg_pose_ifname_.c_str());
78  } else {
79  pos_x_ = config->get_float(AMCL_CFG_PREFIX"map-lasergen/pos_x");
80  pos_y_ = config->get_float(AMCL_CFG_PREFIX"map-lasergen/pos_y");
81  pos_theta_ = config->get_float(AMCL_CFG_PREFIX"map-lasergen/pos_theta");
82  }
83 
84  std::vector<std::pair<int, int> > free_space_indices;
85  map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
86  cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
87  cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
88 
89  map_width_ = map_->size_x;
90  map_height_ = map_->size_y;
91 
92  logger->log_info(name(), "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
93  map_width_, map_height_, free_space_indices.size(),
94  map_width_ * map_height_,
95  (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
96 
97  laser_if_ = blackboard->open_for_writing<Laser360Interface>(cfg_laser_ifname_.c_str());
98  gt_pose_if_ = blackboard->open_for_writing<Position3DInterface>("Map LaserGen Groundtruth");
99 
100  cfg_add_noise_ = false;
101  try {
102  cfg_add_noise_ = config->get_bool(AMCL_CFG_PREFIX"map-lasergen/add_gaussian_noise");
103  } catch (Exception &e) {}; // ignored
104  if (cfg_add_noise_) {
105 #ifndef HAVE_RANDOM
106  throw Exception("Noise has been enabled but C++11 <random> no available at compile time");
107 #else
108  cfg_noise_sigma_ = config->get_float(AMCL_CFG_PREFIX"map-lasergen/noise_sigma");
109  std::random_device rd;
110  noise_rg_ = std::mt19937(rd());
111  noise_nd_ = std::normal_distribution<float>(0.0, cfg_noise_sigma_);
112 #endif
113  }
114 
115  cfg_send_zero_odom_ = false;
116  try {
117  cfg_send_zero_odom_ = config->get_bool(AMCL_CFG_PREFIX"map-lasergen/send_zero_odom");
118  } catch (Exception &e) {}; // ignored
119  if (cfg_send_zero_odom_) {
120  logger->log_info(name(), "Enabling zero odometry publishing");
122  }
123 
124  laser_if_->set_frame(laser_frame_id_.c_str());
125 }
126 
127 
128 void
130 {
131 
132  if (!laser_pose_set_) {
133  if (set_laser_pose()) {
134  laser_pose_set_ = true;
135 
136  if (! cfg_use_current_pose_) {
137  // only need to set it once, let's do it here
138  laser_pos_x_ = pos_x_ + laser_pose_.getOrigin().x();
139  laser_pos_y_ = pos_y_ + laser_pose_.getOrigin().y();
140  laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
141  }
142  } else {
143  if (fawkes::runtime::uptime() >= tf_listener->get_cache_time()) {
144  logger->log_warn(name(), "Could not determine laser pose, skipping loop");
145  }
146  return;
147  }
148  }
149 
150  if (cfg_use_current_pose_) {
151  cur_pose_if_->read();
152  pos_x_ = cur_pose_if_->translation(0);
153  pos_y_ = cur_pose_if_->translation(1);
154  pos_theta_ = tf::get_yaw(tf::Quaternion(cur_pose_if_->rotation(0),
155  cur_pose_if_->rotation(1),
156  cur_pose_if_->rotation(2),
157  cur_pose_if_->rotation(3)));
158 
159  //logger->log_info(name(), "Pos: (%f,%f,%f)", pos_x_, pos_y_, pos_theta_);
160 
161  const float lx = laser_pose_.getOrigin().x();
162  const float ly = laser_pose_.getOrigin().y();
163  laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
164  const float sin_theta = sinf(laser_pos_theta_);
165  const float cos_theta = cosf(laser_pos_theta_);
166  laser_pos_x_ = pos_x_ + (lx * cos_theta - ly * sin_theta);
167  laser_pos_y_ = pos_y_ + (lx * sin_theta + ly * cos_theta);
168  }
169 
170  tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
171  gt_pose_if_->set_translation(0, pos_x_);
172  gt_pose_if_->set_translation(1, pos_y_);
173  gt_pose_if_->set_rotation(0, q.x());
174  gt_pose_if_->set_rotation(1, q.y());
175  gt_pose_if_->set_rotation(2, q.z());
176  gt_pose_if_->set_rotation(3, q.w());
177  gt_pose_if_->write();
178 
179  float dists[360];
180  for (unsigned int i = 0; i < 360; ++i) {
181  dists[i] = map_calc_range(map_, laser_pos_x_, laser_pos_y_,
182  normalize_rad(deg2rad(i) + laser_pos_theta_), 100.);
183  }
184 #ifdef HAVE_RANDOM
185  if (cfg_add_noise_) {
186  for (unsigned int i = 0; i < 360; ++i) {
187  dists[i] += noise_nd_(noise_rg_);
188  }
189  }
190 #endif
191  laser_if_->set_distances(dists);
192  laser_if_->write();
193 
194 
195  if (cfg_send_zero_odom_) {
196  tf::Transform
197  tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0,0,0));
198  Time transform_expiration =
199  (Time(clock) + 1.0);
200 
201  tf::StampedTransform tmp_tf_stamped(tmp_tf,
202  transform_expiration,
203  odom_frame_id_, base_frame_id_);
204  tf_publisher->send_transform(tmp_tf_stamped);
205  }
206 }
207 
209 {
210  if (map_) {
211  map_free(map_);
212  map_ = NULL;
213  }
214 
215  blackboard->close(laser_if_);
216  blackboard->close(gt_pose_if_);
217  blackboard->close(cur_pose_if_);
218 }
219 
220 
221 bool
222 MapLaserGenThread::set_laser_pose()
223 {
224  fawkes::Time now(clock);
226  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
227  &now, laser_frame_id_);
228  try {
229  tf_listener->transform_pose(base_frame_id_, ident, laser_pose_);
230  } catch (fawkes::Exception& e) {
231  return false;
232  }
233 
234  logger->log_debug(name(), "LaserTF: (%f,%f,%f)",
235  laser_pose_.getOrigin().x(), laser_pose_.getOrigin().y(),
236  tf::get_yaw(laser_pose_.getRotation()));
237 
238  return true;
239 }
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:71
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:93
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:91
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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:44
Thread aspect to access the transform system.
Definition: tf.h:42
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
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:36
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:477
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
const char * name() const
Get name of thread.
Definition: thread.h:95
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:133
virtual ~MapLaserGenThread()
Destructor.
void tf_enable_publisher(const char *frame_id=0)
Late enabling of publisher.
Definition: tf.cpp:156
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:37
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system...
Definition: tf.h:70
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
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.