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 <aspect/blackboard.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 <core/threading/thread.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::ClockAspect,
44  public fawkes::LoggingAspect,
49 {
50 public:
52  virtual ~MapLaserGenThread();
53 
54  virtual void init();
55  virtual void loop();
56  virtual void finalize();
57  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
58 protected:
59  virtual void
60  run()
61  {
62  Thread::run();
63  }
64 
65 private:
66  bool set_laser_pose();
67 
68 private:
69  std::string cfg_map_file_;
70  float cfg_resolution_;
71  float cfg_origin_x_;
72  float cfg_origin_y_;
73  float cfg_origin_theta_;
74  float cfg_occupied_thresh_;
75  float cfg_free_thresh_;
76  bool cfg_send_zero_odom_;
77  bool cfg_use_current_pose_;
78 
79  std::string cfg_laser_ifname_;
80  std::string cfg_pose_ifname_;
81  std::string laser_frame_id_;
82  std::string odom_frame_id_;
83  std::string base_frame_id_;
84 
85  unsigned int map_width_;
86  unsigned int map_height_;
87  bool laser_pose_set_;
88 
89  fawkes::tf::Transform latest_tf_;
91 
92  float pos_x_;
93  float pos_y_;
94  float pos_theta_;
95  float laser_pos_x_;
96  float laser_pos_y_;
97  float laser_pos_theta_;
98  map_t *map_;
99 
100  bool cfg_add_noise_;
101  float cfg_noise_sigma_;
102 #ifdef HAVE_RANDOM
103  std::mt19937 noise_rg_;
104  std::normal_distribution<float> noise_nd_;
105 #endif
106 
107  fawkes::Laser360Interface * laser_if_;
108  fawkes::Position3DInterface *gt_pose_if_;
109  fawkes::Position3DInterface *cur_pose_if_;
110 };
111 
112 #endif
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
MapLaserGenThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: map_lasergen_thread.h:60
fawkes::tf::Stamped< fawkes::tf::Pose >
MapLaserGenThread
Generate laser data from map and position.
Definition: map_lasergen_thread.h:49
MapLaserGenThread::~MapLaserGenThread
virtual ~MapLaserGenThread()
Destructor.
Definition: map_lasergen_thread.cpp:45
MapLaserGenThread::finalize
virtual void finalize()
Finalize the thread.
Definition: map_lasergen_thread.cpp:223
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
MapLaserGenThread::MapLaserGenThread
MapLaserGenThread()
Constructor.
Definition: map_lasergen_thread.cpp:36
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::Position3DInterface
Position3DInterface Fawkes BlackBoard Interface.
Definition: Position3DInterface.h:34
MapLaserGenThread::init
virtual void init()
Initialize the thread.
Definition: map_lasergen_thread.cpp:50
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
fawkes::Laser360Interface
Laser360Interface Fawkes BlackBoard Interface.
Definition: Laser360Interface.h:34
MapLaserGenThread::loop
virtual void loop()
Code to execute in the thread.
Definition: map_lasergen_thread.cpp:146
fawkes::ClockAspect
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34