21 #ifndef _PLUGINS_AMCL_AMCL_THREAD_H_
22 #define _PLUGINS_AMCL_AMCL_THREAD_H_
24 #define NEW_UNIFORM_SAMPLING 1
28 #include "pf/pf_vector.h"
29 #include "sensors/amcl_laser.h"
30 #include "sensors/amcl_odom.h"
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>
55 pf_vector_t pf_pose_mean;
57 pf_matrix_t pf_pose_cov;
98 bool set_laser_pose();
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,
109 const fawkes::tf::Pose &pose,
110 const double * covariance);
117 std::string cfg_map_file_;
118 float cfg_resolution_;
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_;
129 std::string cfg_laser_ifname_;
130 std::string cfg_pose_ifname_;
132 unsigned int map_width_;
133 unsigned int map_height_;
134 bool laser_pose_set_;
136 fawkes::tf::Transform latest_tf_;
138 amcl::odom_model_t odom_model_type_;
139 amcl::laser_model_t laser_model_type_;
141 int max_beams_, min_particles_, max_particles_;
143 bool sent_first_transform_;
144 bool latest_tf_valid_;
149 double save_pose_period_;
150 double transform_tolerance_;
158 bool first_map_received_;
159 bool first_reconfigure_call_;
162 double pf_err_, pf_z_;
164 pf_vector_t pf_odom_pose_;
165 double laser_min_range_;
166 double laser_max_range_;
168 amcl::AMCLOdom * odom_;
169 amcl::AMCLLaser *laser_;
171 bool laser_buffered_;
175 double last_covariance_[36];
188 float laser_likelihood_max_dist_;
196 float angle_increment_;
198 unsigned int angle_min_idx_;
199 unsigned int angle_max_idx_;
200 unsigned int angle_range_;
202 unsigned int resample_interval_;
206 std::string odom_frame_id_;
207 std::string base_frame_id_;
208 std::string global_frame_id_;
210 #if NEW_UNIFORM_SAMPLING
211 static std::vector<std::pair<int, int>> free_space_indices;