21 #include "map_lasergen_thread.h" 22 #include "amcl_utils.h" 24 #include <utils/math/angle.h> 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_);
Laser360Interface Fawkes BlackBoard Interface.
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.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
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.
virtual void loop()
Code to execute in the thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
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.
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 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.