Fawkes API  Fawkes Development Version
map_lasergen_thread.cpp
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 }