Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * amcl_thread.cpp - Thread to perform localization 00003 * 00004 * Created: Wed May 16 16:03:38 2012 00005 * Copyright 2012 Tim Niemueller [www.niemueller.de] 00006 ****************************************************************************/ 00007 00008 /* This program is free software; you can redistribute it and/or modify 00009 * it under the terms of the GNU General Public License as published by 00010 * the Free Software Foundation; either version 2 of the License, or 00011 * (at your option) any later version. 00012 * 00013 * This program is distributed in the hope that it will be useful, 00014 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00016 * GNU Library General Public License for more details. 00017 * 00018 * Read the full text in the LICENSE.GPL file in the doc directory. 00019 */ 00020 00021 #ifndef __PLUGINS_AMCL_AMCL_THREAD_H_ 00022 #define __PLUGINS_AMCL_AMCL_THREAD_H_ 00023 00024 #define NEW_UNIFORM_SAMPLING 1 00025 00026 #include "map/map.h" 00027 #include "pf/pf.h" 00028 #include "pf/pf_vector.h" 00029 #include "sensors/amcl_odom.h" 00030 #include "sensors/amcl_laser.h" 00031 00032 #include <core/threading/thread.h> 00033 #include <aspect/blocked_timing.h> 00034 #include <aspect/clock.h> 00035 #include <aspect/configurable.h> 00036 #include <aspect/logging.h> 00037 #include <aspect/tf.h> 00038 #include <aspect/blackboard.h> 00039 00040 #include <interfaces/Laser360Interface.h> 00041 #include <interfaces/Position3DInterface.h> 00042 00043 #include <algorithm> 00044 #include <vector> 00045 #include <map> 00046 #include <cmath> 00047 00048 #ifdef HAVE_ROS 00049 # include <plugins/ros/aspect/ros.h> 00050 # include <ros/publisher.h> 00051 # include <ros/subscriber.h> 00052 # include <geometry_msgs/PoseWithCovarianceStamped.h> 00053 #endif 00054 00055 00056 /// Pose hypothesis 00057 typedef struct { 00058 /// Total weight (weights sum to 1) 00059 double weight; 00060 /// Mean of pose esimate 00061 pf_vector_t pf_pose_mean; 00062 /// Covariance of pose estimate 00063 pf_matrix_t pf_pose_cov; 00064 } amcl_hyp_t; 00065 00066 namespace fawkes { 00067 class Mutex; 00068 } 00069 00070 class AmclThread 00071 : public fawkes::Thread, 00072 public fawkes::ClockAspect, 00073 public fawkes::LoggingAspect, 00074 public fawkes::ConfigurableAspect, 00075 public fawkes::BlockedTimingAspect, 00076 public fawkes::BlackBoardAspect, 00077 #ifdef HAVE_ROS 00078 public fawkes::ROSAspect, 00079 #endif 00080 public fawkes::TransformAspect 00081 { 00082 public: 00083 AmclThread(); 00084 virtual ~AmclThread(); 00085 00086 virtual void init(); 00087 virtual void loop(); 00088 virtual void finalize(); 00089 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00090 protected: virtual void run() { Thread::run();} 00091 00092 private: 00093 bool set_laser_pose(); 00094 bool get_odom_pose(fawkes::tf::Stamped<fawkes::tf::Pose>& odom_pose, 00095 double& x, double& y, double& yaw, 00096 const fawkes::Time* t, const std::string& f); 00097 void apply_initial_pose(); 00098 static pf_vector_t uniform_pose_generator(void* arg); 00099 #ifdef HAVE_ROS 00100 void initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); 00101 # ifdef USE_MAP_PUB 00102 void publish_map(); 00103 # endif 00104 #endif 00105 00106 private: 00107 fawkes::Mutex *conf_mutex_; 00108 00109 std::string cfg_map_file_; 00110 float cfg_resolution_; 00111 float cfg_origin_x_; 00112 float cfg_origin_y_; 00113 float cfg_origin_theta_; 00114 float cfg_occupied_thresh_; 00115 float cfg_free_thresh_; 00116 00117 std::string cfg_laser_ifname_; 00118 std::string cfg_pose_ifname_; 00119 00120 unsigned int map_width_; 00121 unsigned int map_height_; 00122 bool laser_pose_set_; 00123 00124 fawkes::tf::Transform latest_tf_; 00125 00126 amcl::odom_model_t odom_model_type_; 00127 amcl::laser_model_t laser_model_type_; 00128 00129 int max_beams_, min_particles_, max_particles_; 00130 00131 bool sent_first_transform_; 00132 bool latest_tf_valid_; 00133 map_t* map_; 00134 pf_t *pf_; 00135 int resample_count_; 00136 00137 double gui_publish_period; 00138 double save_pose_period; 00139 double cloud_pub_interval; 00140 double transform_tolerance_; 00141 fawkes::Time save_pose_last_time; 00142 00143 fawkes::Laser360Interface* laser_if_; 00144 fawkes::Position3DInterface * pos3d_if_; 00145 00146 #ifdef HAVE_ROS 00147 ros::Publisher pose_pub_; 00148 ros::Publisher particlecloud_pub_; 00149 ros::Subscriber initial_pose_sub_; 00150 ros::Publisher map_pub_; 00151 #endif 00152 00153 amcl_hyp_t* initial_pose_hyp_; 00154 bool first_map_received_; 00155 bool first_reconfigure_call_; 00156 00157 char* mapdata; 00158 int sx, sy; 00159 double resolution; 00160 00161 00162 // Particle filter 00163 double pf_err_, pf_z_; 00164 bool pf_init_; 00165 pf_vector_t pf_odom_pose_; 00166 double laser_min_range_; 00167 double laser_max_range_; 00168 00169 amcl::AMCLOdom* odom_; 00170 amcl::AMCLLaser* laser_; 00171 bool laser_update_; 00172 00173 fawkes::Time last_cloud_pub_time; 00174 fawkes::Time last_laser_received_ts_; 00175 double last_covariance_[36]; 00176 00177 00178 float alpha1_; 00179 float alpha2_; 00180 float alpha3_; 00181 float alpha4_; 00182 float alpha5_; 00183 float z_hit_; 00184 float z_short_; 00185 float z_max_; 00186 float z_rand_; 00187 float sigma_hit_; 00188 float lambda_short_; 00189 float laser_likelihood_max_dist_; 00190 float d_thresh_; 00191 float a_thresh_; 00192 float t_thresh_; 00193 float alpha_slow_; 00194 float alpha_fast_; 00195 float init_pose_[3]; 00196 float init_cov_[3]; 00197 float angle_increment_; 00198 float angle_min_; 00199 unsigned int angle_min_idx_; 00200 unsigned int angle_max_idx_; 00201 unsigned int angle_range_; 00202 00203 unsigned int resample_interval_; 00204 00205 fawkes::Time *last_move_time_; 00206 00207 std::string odom_frame_id_; 00208 std::string base_frame_id_; 00209 std::string global_frame_id_; 00210 std::string laser_frame_id_; 00211 00212 #if NEW_UNIFORM_SAMPLING 00213 static std::vector<std::pair<int,int> > free_space_indices; 00214 #endif 00215 }; 00216 00217 #endif