21 #include "map_lasergen_thread.h" 22 #include "amcl_utils.h" 24 #include <utils/math/angle.h> 25 #include <baseapp/run.h> 36 :
Thread(
"MapLaserGenThread",
Thread::OPMODE_WAITFORWAKEUP),
50 laser_pose_set_ =
false;
52 bool have_custom_map =
false;
55 have_custom_map =
true;
59 have_custom_map ?
"custom" :
"AMCL");
60 fawkes::amcl::read_map_config(
config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
61 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
63 have_custom_map ? AMCL_CFG_PREFIX
"map-lasergen/" : AMCL_CFG_PREFIX);
65 cfg_use_current_pose_ =
false;
67 cfg_use_current_pose_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/use_current_pos");
70 cfg_laser_ifname_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/laser_interface_id");
73 laser_frame_id_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/laser_frame_id");
75 if (cfg_use_current_pose_) {
76 cfg_pose_ifname_ =
config->
get_string(AMCL_CFG_PREFIX
"map-lasergen/pose_interface_id");
81 pos_theta_ =
config->
get_float(AMCL_CFG_PREFIX
"map-lasergen/pos_theta");
84 std::vector<std::pair<int, int> > free_space_indices;
85 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
86 cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
87 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
89 map_width_ = map_->size_x;
90 map_height_ = map_->size_y;
93 map_width_, map_height_, free_space_indices.size(),
94 map_width_ * map_height_,
95 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
100 cfg_add_noise_ =
false;
102 cfg_add_noise_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/add_gaussian_noise");
104 if (cfg_add_noise_) {
106 throw Exception(
"Noise has been enabled but C++11 <random> no available at compile time");
108 cfg_noise_sigma_ =
config->
get_float(AMCL_CFG_PREFIX
"map-lasergen/noise_sigma");
109 std::random_device rd;
110 noise_rg_ = std::mt19937(rd());
111 noise_nd_ = std::normal_distribution<float>(0.0, cfg_noise_sigma_);
115 cfg_send_zero_odom_ =
false;
117 cfg_send_zero_odom_ =
config->
get_bool(AMCL_CFG_PREFIX
"map-lasergen/send_zero_odom");
119 if (cfg_send_zero_odom_) {
124 laser_if_->
set_frame(laser_frame_id_.c_str());
132 if (!laser_pose_set_) {
133 if (set_laser_pose()) {
134 laser_pose_set_ =
true;
136 if (! cfg_use_current_pose_) {
138 laser_pos_x_ = pos_x_ + laser_pose_.getOrigin().x();
139 laser_pos_y_ = pos_y_ + laser_pose_.getOrigin().y();
140 laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
150 if (cfg_use_current_pose_) {
151 cur_pose_if_->
read();
154 pos_theta_ = tf::get_yaw(tf::Quaternion(cur_pose_if_->
rotation(0),
161 const float lx = laser_pose_.getOrigin().x();
162 const float ly = laser_pose_.getOrigin().y();
163 laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose_.getRotation());
164 const float sin_theta = sinf(laser_pos_theta_);
165 const float cos_theta = cosf(laser_pos_theta_);
166 laser_pos_x_ = pos_x_ + (lx * cos_theta - ly * sin_theta);
167 laser_pos_y_ = pos_y_ + (lx * sin_theta + ly * cos_theta);
170 tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_));
177 gt_pose_if_->
write();
180 for (
unsigned int i = 0; i < 360; ++i) {
181 dists[i] = map_calc_range(map_, laser_pos_x_, laser_pos_y_,
185 if (cfg_add_noise_) {
186 for (
unsigned int i = 0; i < 360; ++i) {
187 dists[i] += noise_nd_(noise_rg_);
195 if (cfg_send_zero_odom_) {
197 tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0,0,0));
198 Time transform_expiration =
202 transform_expiration,
203 odom_frame_id_, base_frame_id_);
222 MapLaserGenThread::set_laser_pose()
226 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
227 &now, laser_frame_id_);
235 laser_pose_.getOrigin().x(), laser_pose_.getOrigin().y(),
236 tf::get_yaw(laser_pose_.getRotation()));
Laser360Interface Fawkes BlackBoard Interface.
double * rotation() const
Get rotation value.
virtual void init()
Initialize the thread.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
A class for handling time.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
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.
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
MapLaserGenThread()
Constructor.
Position3DInterface Fawkes BlackBoard Interface.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
const char * name() const
Get name of thread.
virtual void loop()
Code to execute in the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
double * translation() const
Get translation value.
Wrapper class to add time stamp and frame ID to base types.
virtual ~MapLaserGenThread()
Destructor.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void finalize()
Finalize the thread.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.