21 #include "map_lasergen_thread.h"
23 #include "amcl_utils.h"
25 #include <baseapp/run.h>
26 #include <utils/math/angle.h>
37 :
Thread(
"MapLaserGenThread",
Thread::OPMODE_WAITFORWAKEUP),
52 laser_pose_set_ =
false;
54 bool have_custom_map =
false;
57 have_custom_map =
true;
61 logger->
log_info(
name(),
"Using %s map configuration", have_custom_map ?
"custom" :
"AMCL");
62 fawkes::amcl::read_map_config(
config,
70 have_custom_map ? AMCL_CFG_PREFIX
"map-lasergen/"
73 cfg_use_current_pose_ =
false;
75 cfg_use_current_pose_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/use_current_pos");
79 cfg_laser_ifname_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/laser_interface_id");
82 laser_frame_id_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/laser_frame_id");
84 if (cfg_use_current_pose_) {
85 cfg_pose_ifname_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/pose_interface_id");
90 pos_theta_ =
config->
get_float(AMCL_CFG_PREFIX
"map-lasergen/pos_theta");
93 std::vector<std::pair<int, int>> free_space_indices;
94 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
102 map_width_ = map_->size_x;
103 map_height_ = map_->size_y;
106 "Size: %ux%u (%zu of %u cells free, this are %.1f%%)",
109 free_space_indices.size(),
110 map_width_ * map_height_,
111 (
float)free_space_indices.size() / (
float)(map_width_ * map_height_) * 100.);
116 cfg_add_noise_ =
false;
118 cfg_add_noise_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/add_gaussian_noise");
121 if (cfg_add_noise_) {
123 throw Exception(
"Noise has been enabled but C++11 <random> no available at compile time");
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_);
132 cfg_send_zero_odom_ =
false;
134 cfg_send_zero_odom_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/send_zero_odom");
137 if (cfg_send_zero_odom_) {
142 laser_if_->
set_frame(laser_frame_id_.c_str());
148 if (!laser_pose_set_) {
149 if (set_laser_pose()) {
150 laser_pose_set_ =
true;
152 if (!cfg_use_current_pose_) {
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());
166 if (cfg_use_current_pose_) {
167 cur_pose_if_->
read();
170 pos_theta_ = tf::get_yaw(tf::Quaternion(cur_pose_if_->
rotation(0),
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);
186 tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
193 gt_pose_if_->
write();
196 for (
unsigned int i = 0; i < 360; ++i) {
197 dists[i] = map_calc_range(
201 if (cfg_add_noise_) {
202 for (
unsigned int i = 0; i < 360; ++i) {
203 dists[i] += noise_nd_(noise_rg_);
210 if (cfg_send_zero_odom_) {
211 tf::Transform tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0, 0, 0));
215 transform_expiration,
236 MapLaserGenThread::set_laser_pose()
249 "LaserTF: (%f,%f,%f)",
250 laser_pose_.getOrigin().x(),
251 laser_pose_.getOrigin().y(),
252 tf::get_yaw(laser_pose_.getRotation()));