Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 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 
26 using namespace fawkes;
27 
28 /** @class MapLaserGenThread "map_lasergen_thread.h"
29  * Generate laser data from map and position.
30  * @author Tim Niemueller
31  */
32 
33 /** Constructor. */
35  : Thread("MapLaserGenThread", Thread::OPMODE_WAITFORWAKEUP),
36  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
37  TransformAspect(TransformAspect::BOTH, "Odometry")
38 {
39  map_ = NULL;
40 }
41 
42 /** Destructor. */
44 {
45 }
46 
48 {
49  fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
50  cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
51  cfg_free_thresh_);
52 
53  cfg_laser_ifname_ = config->get_string(CFG_PREFIX"laser_interface_id");
54 
55  odom_frame_id_ = config->get_string(CFG_PREFIX"odom_frame_id");
56  base_frame_id_ = config->get_string(CFG_PREFIX"base_frame_id");
57  laser_frame_id_ = config->get_string(CFG_PREFIX"laser_frame_id");
58 
59  std::vector<std::pair<int, int> > free_space_indices;
60  map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
61  cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
62  cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
63 
64  map_width_ = map_->size_x;
65  map_height_ = map_->size_y;
66 
67  logger->log_info(name(), "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
68  map_width_, map_height_, free_space_indices.size(),
69  map_width_ * map_height_,
70  (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
71 
72  laser_if_ = blackboard->open_for_writing<Laser360Interface>(cfg_laser_ifname_.c_str());
73  pos3d_if_ = blackboard->open_for_writing<Position3DInterface>("Map LaserGen Groundtruth");
74 
75  pos_x_ = config->get_float(CFG_PREFIX"map-lasergen/pos_x");
76  pos_y_ = config->get_float(CFG_PREFIX"map-lasergen/pos_y");
77  pos_theta_ = config->get_float(CFG_PREFIX"map-lasergen/pos_theta");
78 
79  cfg_add_noise_ = false;
80  try {
81  cfg_add_noise_ = config->get_bool(CFG_PREFIX"map-lasergen/add_gaussian_noise");
82  } catch (Exception &e) {}; // ignored
83  if (cfg_add_noise_) {
84 #ifndef HAVE_RANDOM
85  throw Exception("Noise has been enabled but C++11 <random> no available at compile time");
86 #else
87  cfg_noise_sigma_ = config->get_float(CFG_PREFIX"map-lasergen/noise_sigma");
88  std::random_device rd;
89  noise_rg_ = std::mt19937(rd());
90  noise_nd_ = std::normal_distribution<float>(0.0, cfg_noise_sigma_);
91 #endif
92  }
93 
94  cfg_send_zero_odom_ = false;
95  try {
96  cfg_send_zero_odom_ = config->get_bool(CFG_PREFIX"map-lasergen/send_zero_odom");
97  } catch (Exception &e) {}; // ignored
98 
99  laser_if_->set_frame(laser_frame_id_.c_str());
100 }
101 
102 
103 void
105 {
106  if (!laser_pose_set_) {
107  if (set_laser_pose()) {
108  laser_pose_set_ = true;
109 
110 
111  tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
112  pos3d_if_->set_translation(0, pos_x_);
113  pos3d_if_->set_translation(1, pos_y_);
114  pos3d_if_->set_rotation(0, q.x());
115  pos3d_if_->set_rotation(1, q.y());
116  pos3d_if_->set_rotation(2, q.z());
117  pos3d_if_->set_rotation(3, q.w());
118  pos3d_if_->write();
119 
120  } else {
121  logger->log_warn(name(), "Could not determine laser pose, skipping loop");
122  return;
123  }
124  }
125 
126  float dists[360];
127  for (unsigned int i = 0; i < 360; ++i) {
128  dists[i] = map_calc_range(map_, laser_pos_x_, laser_pos_y_,
129  normalize_rad(deg2rad(i) + laser_pos_theta_), 100.);
130  }
131 #ifdef HAVE_RANDOM
132  if (cfg_add_noise_) {
133  for (unsigned int i = 0; i < 360; ++i) {
134  dists[i] += noise_nd_(noise_rg_);
135  }
136  }
137 #endif
138  laser_if_->set_distances(dists);
139  laser_if_->write();
140 
141 
142  if (cfg_send_zero_odom_) {
143  tf::Transform
144  tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0,0,0));
145  Time transform_expiration =
146  (Time(clock) + 1.0);
147 
148  tf::StampedTransform tmp_tf_stamped(tmp_tf,
149  transform_expiration,
150  odom_frame_id_, base_frame_id_);
151  tf_publisher->send_transform(tmp_tf_stamped);
152  }
153 }
154 
156 {
157  if (map_) {
158  map_free(map_);
159  map_ = NULL;
160  }
161 
162  blackboard->close(laser_if_);
163  blackboard->close(pos3d_if_);
164 }
165 
166 
167 bool
168 MapLaserGenThread::set_laser_pose()
169 {
170  fawkes::Time now(clock);
172  ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
173  &now, laser_frame_id_);
174  tf::Stamped<tf::Pose> laser_pose;
175  try {
176  tf_listener->transform_pose(base_frame_id_, ident, laser_pose);
177  } catch (fawkes::Exception& e) {
178  return false;
179  }
180 
181  laser_pos_x_ = pos_x_ + laser_pose.getOrigin().x();
182  laser_pos_y_ = pos_y_ + laser_pose.getOrigin().y();
183  laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose.getRotation());
184 
185  logger->log_debug(name(), "Pos: (%f,%f,%f) LaserTF: (%f,%f,%f) LaserPos:(%f,%f,%f)",
186  pos_x_, pos_y_, pos_theta_,
187  laser_pose.getOrigin().x(), laser_pose.getOrigin().y(),
188  tf::get_yaw(laser_pose.getRotation()),
189  laser_pos_x_, laser_pos_y_, laser_pos_theta_);
190 
191  return true;
192 }