Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
40 #include <interfaces/Laser360Interface.h>
41 #include <interfaces/Position3DInterface.h>
42 
43 #include <algorithm>
44 #include <vector>
45 #include <map>
46 #include <cmath>
47 
48 #ifdef HAVE_ROS
49 # include <plugins/ros/aspect/ros.h>
50 # include <ros/publisher.h>
51 # include <ros/subscriber.h>
52 # include <geometry_msgs/PoseWithCovarianceStamped.h>
53 #endif
54 
55 
56 /// Pose hypothesis
57 typedef struct {
58  /// Total weight (weights sum to 1)
59  double weight;
60  /// Mean of pose esimate
61  pf_vector_t pf_pose_mean;
62  /// Covariance of pose estimate
63  pf_matrix_t pf_pose_cov;
64 } amcl_hyp_t;
65 
66 namespace fawkes {
67  class Mutex;
68 }
69 
71 : public fawkes::Thread,
72  public fawkes::ClockAspect,
73  public fawkes::LoggingAspect,
77 #ifdef HAVE_ROS
78  public fawkes::ROSAspect,
79 #endif
81 {
82 public:
83  AmclThread();
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: virtual void run() { Thread::run();}
91 
92  private:
93  bool set_laser_pose();
94  bool get_odom_pose(fawkes::tf::Stamped<fawkes::tf::Pose>& odom_pose,
95  double& x, double& y, double& yaw,
96  const fawkes::Time* t, const std::string& f);
97  void apply_initial_pose();
98  static pf_vector_t uniform_pose_generator(void* arg);
99 #ifdef HAVE_ROS
100  void initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
101 # ifdef USE_MAP_PUB
102  void publish_map();
103 # endif
104 #endif
105 
106 private:
107  fawkes::Mutex *conf_mutex_;
108 
109  std::string cfg_map_file_;
110  float cfg_resolution_;
111  float cfg_origin_x_;
112  float cfg_origin_y_;
113  float cfg_origin_theta_;
114  float cfg_occupied_thresh_;
115  float cfg_free_thresh_;
116 
117  std::string cfg_laser_ifname_;
118  std::string cfg_pose_ifname_;
119 
120  unsigned int map_width_;
121  unsigned int map_height_;
122  bool laser_pose_set_;
123 
124  fawkes::tf::Transform latest_tf_;
125 
126  amcl::odom_model_t odom_model_type_;
127  amcl::laser_model_t laser_model_type_;
128 
129  int max_beams_, min_particles_, max_particles_;
130 
131  bool sent_first_transform_;
132  bool latest_tf_valid_;
133  map_t* map_;
134  pf_t *pf_;
135  int resample_count_;
136 
137  double gui_publish_period;
138  double save_pose_period;
139  double cloud_pub_interval;
140  double transform_tolerance_;
141  fawkes::Time save_pose_last_time;
142 
143  fawkes::Laser360Interface* laser_if_;
144  fawkes::Position3DInterface * pos3d_if_;
145 
146 #ifdef HAVE_ROS
147  ros::Publisher pose_pub_;
148  ros::Publisher particlecloud_pub_;
149  ros::Subscriber initial_pose_sub_;
150  ros::Publisher map_pub_;
151 #endif
152 
153  amcl_hyp_t* initial_pose_hyp_;
154  bool first_map_received_;
155  bool first_reconfigure_call_;
156 
157  char* mapdata;
158  int sx, sy;
159  double resolution;
160 
161 
162  // Particle filter
163  double pf_err_, pf_z_;
164  bool pf_init_;
165  pf_vector_t pf_odom_pose_;
166  double laser_min_range_;
167  double laser_max_range_;
168 
169  amcl::AMCLOdom* odom_;
170  amcl::AMCLLaser* laser_;
171  bool laser_update_;
172 
173  fawkes::Time last_cloud_pub_time;
174  fawkes::Time last_laser_received_ts_;
175  double last_covariance_[36];
176 
177 
178  float alpha1_;
179  float alpha2_;
180  float alpha3_;
181  float alpha4_;
182  float alpha5_;
183  float z_hit_;
184  float z_short_;
185  float z_max_;
186  float z_rand_;
187  float sigma_hit_;
188  float lambda_short_;
189  float laser_likelihood_max_dist_;
190  float d_thresh_;
191  float a_thresh_;
192  float t_thresh_;
193  float alpha_slow_;
194  float alpha_fast_;
195  float init_pose_[3];
196  float init_cov_[3];
197  float angle_increment_;
198  float angle_min_;
199  unsigned int angle_min_idx_;
200  unsigned int angle_max_idx_;
201  unsigned int angle_range_;
202 
203  unsigned int resample_interval_;
204 
205  fawkes::Time *last_move_time_;
206 
207  std::string odom_frame_id_;
208  std::string base_frame_id_;
209  std::string global_frame_id_;
210  std::string laser_frame_id_;
211 
212 #if NEW_UNIFORM_SAMPLING
213  static std::vector<std::pair<int,int> > free_space_indices;
214 #endif
215 };
216 
217 #endif
Laser360Interface Fawkes BlackBoard Interface.
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
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Thread aspect to access the transform system.
Definition: tf.h:38
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:61
Thread aspect to use blocked timing.
Thread to perform Adaptive Monte Carlo Localization.
Definition: amcl_thread.h:70
virtual void init()
Initialize the thread.
Definition: amcl_thread.cpp:90
Position3DInterface Fawkes BlackBoard Interface.
Pose hypothesis.
Definition: amcl_thread.h:57
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
Definition: amcl_thread.h:63
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:90
Thread aspect to access configuration data.
Definition: configurable.h:35
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:128
AmclThread()
Constructor.
Definition: amcl_thread.cpp:75
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
Definition: amcl_thread.cpp:85
double weight
Total weight (weights sum to 1)
Definition: amcl_thread.h:59
Mutex mutual exclusion lock.
Definition: mutex.h:32
virtual void loop()
Code to execute in the thread.