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
50typedef 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;
59
60namespace fawkes {
61class Mutex;
62}
63
64#ifdef HAVE_ROS
65class AmclROSThread;
66#endif
67
76
77{
78public:
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() */
90protected:
91 virtual void
93 {
94 Thread::run();
95 }
96
97private:
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
114private:
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