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:33:38 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 #ifndef __PLUGINS_AMCL_MAP_LASERGEN_THREAD_H_ 00022 #define __PLUGINS_AMCL_MAP_LASERGEN_THREAD_H_ 00023 00024 #include "map/map.h" 00025 00026 #include <core/threading/thread.h> 00027 #include <aspect/blocked_timing.h> 00028 #include <aspect/clock.h> 00029 #include <aspect/configurable.h> 00030 #include <aspect/logging.h> 00031 #include <aspect/tf.h> 00032 #include <aspect/blackboard.h> 00033 00034 #if __cplusplus > 201100L || defined(__GXX_EXPERIMENTAL_CXX0X__) 00035 # define HAVE_RANDOM 00036 # include <random> 00037 #endif 00038 00039 #include <interfaces/Laser360Interface.h> 00040 #include <interfaces/Position3DInterface.h> 00041 00042 class MapLaserGenThread 00043 : public fawkes::Thread, 00044 public fawkes::ClockAspect, 00045 public fawkes::LoggingAspect, 00046 public fawkes::ConfigurableAspect, 00047 public fawkes::BlockedTimingAspect, 00048 public fawkes::BlackBoardAspect, 00049 public fawkes::TransformAspect 00050 { 00051 public: 00052 MapLaserGenThread(); 00053 virtual ~MapLaserGenThread(); 00054 00055 virtual void init(); 00056 virtual void loop(); 00057 virtual void finalize(); 00058 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00059 protected: virtual void run() { Thread::run();} 00060 00061 private: 00062 bool set_laser_pose(); 00063 00064 private: 00065 std::string cfg_map_file_; 00066 float cfg_resolution_; 00067 float cfg_origin_x_; 00068 float cfg_origin_y_; 00069 float cfg_origin_theta_; 00070 float cfg_occupied_thresh_; 00071 float cfg_free_thresh_; 00072 bool cfg_send_zero_odom_; 00073 00074 std::string cfg_laser_ifname_; 00075 std::string laser_frame_id_; 00076 std::string odom_frame_id_; 00077 std::string base_frame_id_; 00078 00079 unsigned int map_width_; 00080 unsigned int map_height_; 00081 bool laser_pose_set_; 00082 00083 fawkes::tf::Transform latest_tf_; 00084 00085 float pos_x_; 00086 float pos_y_; 00087 float pos_theta_; 00088 float laser_pos_x_; 00089 float laser_pos_y_; 00090 float laser_pos_theta_; 00091 map_t* map_; 00092 00093 bool cfg_add_noise_; 00094 float cfg_noise_sigma_; 00095 #ifdef HAVE_RANDOM 00096 std::mt19937 noise_rg_; 00097 std::normal_distribution<float> noise_nd_; 00098 #endif 00099 00100 fawkes::Laser360Interface* laser_if_; 00101 fawkes::Position3DInterface * pos3d_if_; 00102 }; 00103 00104 #endif