Fawkes API  Fawkes Development Version
amcl_thread.h
1 /***************************************************************************
2  * amcl_thread.cpp - Thread to perform localization
3  *
4  * Created: Wed May 16 16:03:38 2012
5  * Copyright 2012 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_AMCL_THREAD_H_
22 #define _PLUGINS_AMCL_AMCL_THREAD_H_
23 
24 #define NEW_UNIFORM_SAMPLING 1
25 
26 #include "map/map.h"
27 #include "pf/pf.h"
28 #include "pf/pf_vector.h"
29 #include "sensors/amcl_laser.h"
30 #include "sensors/amcl_odom.h"
31 
32 #include <aspect/blackboard.h>
33 #include <aspect/blocked_timing.h>
34 #include <aspect/clock.h>
35 #include <aspect/configurable.h>
36 #include <aspect/logging.h>
37 #include <aspect/tf.h>
38 #include <blackboard/interface_listener.h>
39 #include <core/threading/thread.h>
40 #include <interfaces/Laser360Interface.h>
41 #include <interfaces/LocalizationInterface.h>
42 #include <interfaces/Position3DInterface.h>
43 
44 #include <algorithm>
45 #include <cmath>
46 #include <map>
47 #include <vector>
48 
49 /// Pose hypothesis
50 typedef struct
51 {
52  /// Total weight (weights sum to 1)
53  double weight;
54  /// Mean of pose esimate
55  pf_vector_t pf_pose_mean;
56  /// Covariance of pose estimate
57  pf_matrix_t pf_pose_cov;
58 } amcl_hyp_t;
59 
60 namespace fawkes {
61 class Mutex;
62 }
63 
64 #ifdef HAVE_ROS
65 class AmclROSThread;
66 #endif
67 
68 class AmclThread : public fawkes::Thread,
69  public fawkes::ClockAspect,
70  public fawkes::LoggingAspect,
76 
77 {
78 public:
79 #ifdef HAVE_ROS
80  AmclThread(AmclROSThread *ros_thread);
81 #else
82  AmclThread();
83 #endif
84  virtual ~AmclThread();
85 
86  virtual void init();
87  virtual void loop();
88  virtual void finalize();
89  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
90 protected:
91  virtual void
92  run()
93  {
94  Thread::run();
95  }
96 
97 private:
98  bool set_laser_pose();
99  bool get_odom_pose(fawkes::tf::Stamped<fawkes::tf::Pose> &odom_pose,
100  double & x,
101  double & y,
102  double & yaw,
103  const fawkes::Time * t,
104  const std::string & f);
105  void apply_initial_pose();
106  static pf_vector_t uniform_pose_generator(void *arg);
107  void set_initial_pose(const std::string & frame_id,
108  const fawkes::Time & msg_time,
109  const fawkes::tf::Pose &pose,
110  const double * covariance);
111  virtual bool bb_interface_message_received(fawkes::Interface *interface,
112  fawkes::Message * message) noexcept;
113 
114 private:
115  fawkes::Mutex *conf_mutex_;
116 
117  std::string cfg_map_file_;
118  float cfg_resolution_;
119  float cfg_origin_x_;
120  float cfg_origin_y_;
121  float cfg_origin_theta_;
122  float cfg_occupied_thresh_;
123  float cfg_free_thresh_;
124  bool cfg_read_init_cov_;
125  bool cfg_buffer_enable_;
126  bool cfg_buffer_debug_;
127  bool cfg_use_latest_odom_;
128 
129  std::string cfg_laser_ifname_;
130  std::string cfg_pose_ifname_;
131 
132  unsigned int map_width_;
133  unsigned int map_height_;
134  bool laser_pose_set_;
135 
136  fawkes::tf::Transform latest_tf_;
137 
138  amcl::odom_model_t odom_model_type_;
139  amcl::laser_model_t laser_model_type_;
140 
141  int max_beams_, min_particles_, max_particles_;
142 
143  bool sent_first_transform_;
144  bool latest_tf_valid_;
145  map_t *map_;
146  pf_t * pf_;
147  int resample_count_;
148 
149  double save_pose_period_;
150  double transform_tolerance_;
151  fawkes::Time save_pose_last_time;
152 
153  fawkes::Laser360Interface * laser_if_;
154  fawkes::Position3DInterface * pos3d_if_;
156 
157  amcl_hyp_t *initial_pose_hyp_;
158  bool first_map_received_;
159  bool first_reconfigure_call_;
160 
161  // Particle filter
162  double pf_err_, pf_z_;
163  bool pf_init_;
164  pf_vector_t pf_odom_pose_;
165  double laser_min_range_;
166  double laser_max_range_;
167 
168  amcl::AMCLOdom * odom_;
169  amcl::AMCLLaser *laser_;
170  bool laser_update_;
171  bool laser_buffered_;
172 
173  fawkes::Time last_cloud_pub_time;
174  fawkes::Time last_laser_received_ts_;
175  double last_covariance_[36];
176 
177  float alpha1_;
178  float alpha2_;
179  float alpha3_;
180  float alpha4_;
181  float alpha5_;
182  float z_hit_;
183  float z_short_;
184  float z_max_;
185  float z_rand_;
186  float sigma_hit_;
187  float lambda_short_;
188  float laser_likelihood_max_dist_;
189  float d_thresh_;
190  float a_thresh_;
191  float t_thresh_;
192  float alpha_slow_;
193  float alpha_fast_;
194  float init_pose_[3];
195  float init_cov_[3];
196  float angle_increment_;
197  float angle_min_;
198  unsigned int angle_min_idx_;
199  unsigned int angle_max_idx_;
200  unsigned int angle_range_;
201 
202  unsigned int resample_interval_;
203 
204  fawkes::Time *last_move_time_;
205 
206  std::string odom_frame_id_;
207  std::string base_frame_id_;
208  std::string global_frame_id_;
209 
210 #if NEW_UNIFORM_SAMPLING
211  static std::vector<std::pair<int, int>> free_space_indices;
212 #endif
213 
214 #ifdef HAVE_ROS
215  AmclROSThread *rt_;
216 #endif
217 };
218 
219 #endif
Thread for ROS integration of the Adaptive Monte Carlo Localization.
Definition: ros_thread.h:53
Thread to perform Adaptive Monte Carlo Localization.
Definition: amcl_thread.h:77
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
AmclThread()
Constructor.
Definition: amcl_thread.cpp:83
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: amcl_thread.h:92
virtual ~AmclThread()
Destructor.
Definition: amcl_thread.cpp:98
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
BlackBoard interface listener.
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
Laser360Interface Fawkes BlackBoard Interface.
LocalizationInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:33
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
Mutex mutual exclusion lock.
Definition: mutex.h:33
Position3DInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.
Pose hypothesis.
Definition: amcl_thread.h:51
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:55
double weight
Total weight (weights sum to 1)
Definition: amcl_thread.h:53
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
Definition: amcl_thread.h:57