Fawkes API  Fawkes Development Version
map_lasergen_thread.h
1 /***************************************************************************
2  * map_lasergen_thread.cpp - Thread to generate laser data from map
3  *
4  * Created: Thu Aug 23 18:33:38 2012
5  * Copyright 2012-2014 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #ifndef __PLUGINS_AMCL_MAP_LASERGEN_THREAD_H_
22 #define __PLUGINS_AMCL_MAP_LASERGEN_THREAD_H_
23 
24 #include "map/map.h"
25 
26 #include <core/threading/thread.h>
27 #include <aspect/blocked_timing.h>
28 #include <aspect/clock.h>
29 #include <aspect/configurable.h>
30 #include <aspect/logging.h>
31 #include <aspect/tf.h>
32 #include <aspect/blackboard.h>
33 
34 #if __cplusplus > 201100L || defined(__GXX_EXPERIMENTAL_CXX0X__)
35 # define HAVE_RANDOM
36 # include <random>
37 #endif
38 
39 #include <interfaces/Laser360Interface.h>
40 #include <interfaces/Position3DInterface.h>
41 
43 : public fawkes::Thread,
44  public fawkes::ClockAspect,
45  public fawkes::LoggingAspect,
50 {
51 public:
53  virtual ~MapLaserGenThread();
54 
55  virtual void init();
56  virtual void loop();
57  virtual void finalize();
58  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
59  protected: virtual void run() { Thread::run();}
60 
61  private:
62  bool set_laser_pose();
63 
64  private:
65  std::string cfg_map_file_;
66  float cfg_resolution_;
67  float cfg_origin_x_;
68  float cfg_origin_y_;
69  float cfg_origin_theta_;
70  float cfg_occupied_thresh_;
71  float cfg_free_thresh_;
72  bool cfg_send_zero_odom_;
73  bool cfg_use_current_pose_;
74 
75  std::string cfg_laser_ifname_;
76  std::string cfg_pose_ifname_;
77  std::string laser_frame_id_;
78  std::string odom_frame_id_;
79  std::string base_frame_id_;
80 
81  unsigned int map_width_;
82  unsigned int map_height_;
83  bool laser_pose_set_;
84 
85  fawkes::tf::Transform latest_tf_;
87 
88  float pos_x_;
89  float pos_y_;
90  float pos_theta_;
91  float laser_pos_x_;
92  float laser_pos_y_;
93  float laser_pos_theta_;
94  map_t* map_;
95 
96  bool cfg_add_noise_;
97  float cfg_noise_sigma_;
98 #ifdef HAVE_RANDOM
99  std::mt19937 noise_rg_;
100  std::normal_distribution<float> noise_nd_;
101 #endif
102 
103  fawkes::Laser360Interface* laser_if_;
104  fawkes::Position3DInterface * gt_pose_if_;
105  fawkes::Position3DInterface * cur_pose_if_;
106 };
107 
108 #endif
Laser360Interface Fawkes BlackBoard Interface.
virtual void init()
Initialize the thread.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Generate laser data from map and position.
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
MapLaserGenThread()
Constructor.
Position3DInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:35
virtual void loop()
Code to execute in the thread.
Thread aspect to access configuration data.
Definition: configurable.h:35
virtual ~MapLaserGenThread()
Destructor.
virtual void finalize()
Finalize the thread.