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>
40 #include <interfaces/Laser360Interface.h>
41 #include <interfaces/Position3DInterface.h>
49 # include <plugins/ros/aspect/ros.h>
50 # include <ros/publisher.h>
51 # include <ros/subscriber.h>
52 # include <geometry_msgs/PoseWithCovarianceStamped.h>
90 protected:
virtual void run() { Thread::run();}
93 bool set_laser_pose();
95 double& x,
double& y,
double& yaw,
97 void apply_initial_pose();
98 static pf_vector_t uniform_pose_generator(
void* arg);
100 void initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
109 std::string cfg_map_file_;
110 float cfg_resolution_;
113 float cfg_origin_theta_;
114 float cfg_occupied_thresh_;
115 float cfg_free_thresh_;
117 std::string cfg_laser_ifname_;
118 std::string cfg_pose_ifname_;
120 unsigned int map_width_;
121 unsigned int map_height_;
122 bool laser_pose_set_;
124 fawkes::tf::Transform latest_tf_;
126 amcl::odom_model_t odom_model_type_;
127 amcl::laser_model_t laser_model_type_;
129 int max_beams_, min_particles_, max_particles_;
131 bool sent_first_transform_;
132 bool latest_tf_valid_;
137 double gui_publish_period;
138 double save_pose_period;
139 double cloud_pub_interval;
140 double transform_tolerance_;
147 ros::Publisher pose_pub_;
148 ros::Publisher particlecloud_pub_;
149 ros::Subscriber initial_pose_sub_;
150 ros::Publisher map_pub_;
154 bool first_map_received_;
155 bool first_reconfigure_call_;
163 double pf_err_, pf_z_;
165 pf_vector_t pf_odom_pose_;
166 double laser_min_range_;
167 double laser_max_range_;
169 amcl::AMCLOdom* odom_;
170 amcl::AMCLLaser* laser_;
175 double last_covariance_[36];
189 float laser_likelihood_max_dist_;
197 float angle_increment_;
199 unsigned int angle_min_idx_;
200 unsigned int angle_max_idx_;
201 unsigned int angle_range_;
203 unsigned int resample_interval_;
207 std::string odom_frame_id_;
208 std::string base_frame_id_;
209 std::string global_frame_id_;
210 std::string laser_frame_id_;
212 #if NEW_UNIFORM_SAMPLING
213 static std::vector<std::pair<int,int> > free_space_indices;