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_odom.h" 30 #include "sensors/amcl_laser.h" 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> 41 #include <interfaces/Laser360Interface.h> 42 #include <interfaces/Position3DInterface.h> 43 #include <interfaces/LocalizationInterface.h> 89 virtual void finalize();
94 bool set_laser_pose();
96 double& x,
double& y,
double& yaw,
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);
108 std::string cfg_map_file_;
109 float cfg_resolution_;
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_;
119 std::string cfg_laser_ifname_;
120 std::string cfg_pose_ifname_;
122 unsigned int map_width_;
123 unsigned int map_height_;
124 bool laser_pose_set_;
126 fawkes::tf::Transform latest_tf_;
128 amcl::odom_model_t odom_model_type_;
129 amcl::laser_model_t laser_model_type_;
131 int max_beams_, min_particles_, max_particles_;
133 bool sent_first_transform_;
134 bool latest_tf_valid_;
139 double save_pose_period_;
140 double transform_tolerance_;
148 bool first_map_received_;
149 bool first_reconfigure_call_;
152 double pf_err_, pf_z_;
154 pf_vector_t pf_odom_pose_;
155 double laser_min_range_;
156 double laser_max_range_;
158 amcl::AMCLOdom* odom_;
159 amcl::AMCLLaser* laser_;
161 bool laser_buffered_;
165 double last_covariance_[36];
178 float laser_likelihood_max_dist_;
186 float angle_increment_;
188 unsigned int angle_min_idx_;
189 unsigned int angle_max_idx_;
190 unsigned int angle_range_;
192 unsigned int resample_interval_;
196 std::string odom_frame_id_;
197 std::string base_frame_id_;
198 std::string global_frame_id_;
200 #if NEW_UNIFORM_SAMPLING 201 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.
virtual void run()
Code to execute in the thread.
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.
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.
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.