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;
Laser360Interface Fawkes BlackBoard Interface.
Thread aspect to access to BlackBoard.
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to get access to a ROS node handle.
A class for handling time.
Thread class encapsulation of pthreads.
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.
Wrapper class to add time stamp and frame ID to base types.
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
double weight
Total weight (weights sum to 1)
Mutex mutual exclusion lock.
virtual void loop()
Code to execute in the thread.