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,
236MapLaserGenThread::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()));
virtual ~MapLaserGenThread()
Destructor.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
MapLaserGenThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
Base class for exceptions in Fawkes.
void write()
Write from local copy into BlackBoard memory.
void read()
Read from BlackBoard into local copy.
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Position3DInterface Fawkes BlackBoard Interface.
double * rotation() const
Get rotation value.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
double * translation() const
Get translation value.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).