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