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> 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;
Laser360Interface Fawkes BlackBoard Interface.
LocalizationInterface Fawkes BlackBoard Interface.
Thread aspect to access to BlackBoard.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
Fawkes library namespace.
A class for handling time.
Thread class encapsulation of pthreads.
Base class for all Fawkes BlackBoard interfaces.
pf_vector_t pf_pose_mean
Mean of pose esimate.
Thread aspect to use blocked timing.
Thread to perform Adaptive Monte Carlo Localization.
virtual void init()
Initialize the thread.
Position3DInterface Fawkes BlackBoard Interface.
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
Thread aspect to log output.
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect to access configuration data.
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
double weight
Total weight (weights sum to 1)
Thread for ROS integration of the Adaptive Monte Carlo Localization.
Mutex mutual exclusion lock.
BlackBoard interface listener.
virtual void loop()
Code to execute in the thread.