Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * map_lasergen_thread.cpp - Thread to generate laser data from map 00003 * 00004 * Created: Thu Aug 23 18:43:39 2012 00005 * Copyright 2012 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU Library General Public License for more details. 00017 * 00018 * Read the full text in the LICENSE.GPL file in the doc directory. 00019 */ 00020 00021 #include "map_lasergen_thread.h" 00022 #include "amcl_utils.h" 00023 00024 #include <utils/math/angle.h> 00025 00026 using namespace fawkes; 00027 00028 /** @class MapLaserGenThread "map_lasergen_thread.h" 00029 * Generate laser data from map and position. 00030 * @author Tim Niemueller 00031 */ 00032 00033 /** Constructor. */ 00034 MapLaserGenThread::MapLaserGenThread() 00035 : Thread("MapLaserGenThread", Thread::OPMODE_WAITFORWAKEUP), 00036 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE), 00037 TransformAspect(TransformAspect::BOTH, "Odometry") 00038 { 00039 map_ = NULL; 00040 } 00041 00042 /** Destructor. */ 00043 MapLaserGenThread::~MapLaserGenThread() 00044 { 00045 } 00046 00047 void MapLaserGenThread::init() 00048 { 00049 fawkes::amcl::read_map_config(config, cfg_map_file_, cfg_resolution_, cfg_origin_x_, 00050 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_, 00051 cfg_free_thresh_); 00052 00053 cfg_laser_ifname_ = config->get_string(CFG_PREFIX"laser_interface_id"); 00054 00055 odom_frame_id_ = config->get_string(CFG_PREFIX"odom_frame_id"); 00056 base_frame_id_ = config->get_string(CFG_PREFIX"base_frame_id"); 00057 laser_frame_id_ = config->get_string(CFG_PREFIX"laser_frame_id"); 00058 00059 std::vector<std::pair<int, int> > free_space_indices; 00060 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(), 00061 cfg_origin_x_, cfg_origin_y_, cfg_resolution_, 00062 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices); 00063 00064 map_width_ = map_->size_x; 00065 map_height_ = map_->size_y; 00066 00067 logger->log_info(name(), "Size: %ux%u (%zu of %u cells free, this are %.1f%%)", 00068 map_width_, map_height_, free_space_indices.size(), 00069 map_width_ * map_height_, 00070 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.); 00071 00072 laser_if_ = blackboard->open_for_writing<Laser360Interface>(cfg_laser_ifname_.c_str()); 00073 pos3d_if_ = blackboard->open_for_writing<Position3DInterface>("Map LaserGen Groundtruth"); 00074 00075 pos_x_ = config->get_float(CFG_PREFIX"map-lasergen/pos_x"); 00076 pos_y_ = config->get_float(CFG_PREFIX"map-lasergen/pos_y"); 00077 pos_theta_ = config->get_float(CFG_PREFIX"map-lasergen/pos_theta"); 00078 00079 cfg_add_noise_ = false; 00080 try { 00081 cfg_add_noise_ = config->get_bool(CFG_PREFIX"map-lasergen/add_gaussian_noise"); 00082 } catch (Exception &e) {}; // ignored 00083 if (cfg_add_noise_) { 00084 #ifndef HAVE_RANDOM 00085 throw Exception("Noise has been enabled but C++11 <random> no available at compile time"); 00086 #else 00087 cfg_noise_sigma_ = config->get_float(CFG_PREFIX"map-lasergen/noise_sigma"); 00088 std::random_device rd; 00089 noise_rg_ = std::mt19937(rd()); 00090 noise_nd_ = std::normal_distribution<float>(0.0, cfg_noise_sigma_); 00091 #endif 00092 } 00093 00094 cfg_send_zero_odom_ = false; 00095 try { 00096 cfg_send_zero_odom_ = config->get_bool(CFG_PREFIX"map-lasergen/send_zero_odom"); 00097 } catch (Exception &e) {}; // ignored 00098 00099 laser_if_->set_frame(laser_frame_id_.c_str()); 00100 } 00101 00102 00103 void 00104 MapLaserGenThread::loop() 00105 { 00106 if (!laser_pose_set_) { 00107 if (set_laser_pose()) { 00108 laser_pose_set_ = true; 00109 00110 00111 tf::Quaternion q(tf::create_quaternion_from_yaw(pos_theta_)); 00112 pos3d_if_->set_translation(0, pos_x_); 00113 pos3d_if_->set_translation(1, pos_y_); 00114 pos3d_if_->set_rotation(0, q.x()); 00115 pos3d_if_->set_rotation(1, q.y()); 00116 pos3d_if_->set_rotation(2, q.z()); 00117 pos3d_if_->set_rotation(3, q.w()); 00118 pos3d_if_->write(); 00119 00120 } else { 00121 logger->log_warn(name(), "Could not determine laser pose, skipping loop"); 00122 return; 00123 } 00124 } 00125 00126 float dists[360]; 00127 for (unsigned int i = 0; i < 360; ++i) { 00128 dists[i] = map_calc_range(map_, laser_pos_x_, laser_pos_y_, 00129 normalize_rad(deg2rad(i) + laser_pos_theta_), 100.); 00130 } 00131 #ifdef HAVE_RANDOM 00132 if (cfg_add_noise_) { 00133 for (unsigned int i = 0; i < 360; ++i) { 00134 dists[i] += noise_nd_(noise_rg_); 00135 } 00136 } 00137 #endif 00138 laser_if_->set_distances(dists); 00139 laser_if_->write(); 00140 00141 00142 if (cfg_send_zero_odom_) { 00143 tf::Transform 00144 tmp_tf(tf::create_quaternion_from_yaw(0), tf::Vector3(0,0,0)); 00145 Time transform_expiration = 00146 (Time(clock) + 1.0); 00147 00148 tf::StampedTransform tmp_tf_stamped(tmp_tf, 00149 transform_expiration, 00150 odom_frame_id_, base_frame_id_); 00151 tf_publisher->send_transform(tmp_tf_stamped); 00152 } 00153 } 00154 00155 void MapLaserGenThread::finalize() 00156 { 00157 if (map_) { 00158 map_free(map_); 00159 map_ = NULL; 00160 } 00161 00162 blackboard->close(laser_if_); 00163 blackboard->close(pos3d_if_); 00164 } 00165 00166 00167 bool 00168 MapLaserGenThread::set_laser_pose() 00169 { 00170 fawkes::Time now(clock); 00171 tf::Stamped<tf::Pose> 00172 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)), 00173 &now, laser_frame_id_); 00174 tf::Stamped<tf::Pose> laser_pose; 00175 try { 00176 tf_listener->transform_pose(base_frame_id_, ident, laser_pose); 00177 } catch (fawkes::Exception& e) { 00178 return false; 00179 } 00180 00181 laser_pos_x_ = pos_x_ + laser_pose.getOrigin().x(); 00182 laser_pos_y_ = pos_y_ + laser_pose.getOrigin().y(); 00183 laser_pos_theta_ = pos_theta_ + tf::get_yaw(laser_pose.getRotation()); 00184 00185 logger->log_debug(name(), "Pos: (%f,%f,%f) LaserTF: (%f,%f,%f) LaserPos:(%f,%f,%f)", 00186 pos_x_, pos_y_, pos_theta_, 00187 laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), 00188 tf::get_yaw(laser_pose.getRotation()), 00189 laser_pos_x_, laser_pos_y_, laser_pos_theta_); 00190 00191 return true; 00192 }