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