21 #include "map_lasergen_thread.h"
22 #include "amcl_utils.h"
24 #include <utils/math/angle.h>
26 using namespace fawkes;
35 :
Thread(
"MapLaserGenThread",
Thread::OPMODE_WAITFORWAKEUP),
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_,
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);
64 map_width_ = map_->size_x;
65 map_height_ = map_->size_y;
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.);
79 cfg_add_noise_ =
false;
81 cfg_add_noise_ =
config->
get_bool(CFG_PREFIX
"map-lasergen/add_gaussian_noise");
85 throw Exception(
"Noise has been enabled but C++11 <random> no available at compile time");
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_);
94 cfg_send_zero_odom_ =
false;
96 cfg_send_zero_odom_ =
config->
get_bool(CFG_PREFIX
"map-lasergen/send_zero_odom");
99 laser_if_->
set_frame(laser_frame_id_.c_str());
106 if (!laser_pose_set_) {
107 if (set_laser_pose()) {
108 laser_pose_set_ =
true;
111 tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
127 for (
unsigned int i = 0; i < 360; ++i) {
128 dists[i] = map_calc_range(map_, laser_pos_x_, laser_pos_y_,
132 if (cfg_add_noise_) {
133 for (
unsigned int i = 0; i < 360; ++i) {
134 dists[i] += noise_nd_(noise_rg_);
142 if (cfg_send_zero_odom_) {
144 tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0,0,0));
145 Time transform_expiration =
149 transform_expiration,
150 odom_frame_id_, base_frame_id_);
168 MapLaserGenThread::set_laser_pose()
172 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
173 &now, laser_frame_id_);
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());
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_);