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