$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * This library is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU Lesser General Public 00007 * License as published by the Free Software Foundation; either 00008 * version 2.1 of the License, or (at your option) any later version. 00009 * 00010 * This library is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00013 * Lesser General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU Lesser General Public 00016 * License along with this library; if not, write to the Free Software 00017 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00018 * 00019 */ 00020 00021 /* Author: Brian Gerkey */ 00022 00023 #include <algorithm> 00024 #include <vector> 00025 #include <map> 00026 00027 #include <boost/bind.hpp> 00028 #include <boost/thread/mutex.hpp> 00029 00030 #include "map/map.h" 00031 #include "pf/pf.h" 00032 #include "sensors/amcl_odom.h" 00033 #include "sensors/amcl_laser.h" 00034 00035 #include "ros/assert.h" 00036 00037 // roscpp 00038 #include "ros/ros.h" 00039 00040 // Messages that I need 00041 #include "sensor_msgs/LaserScan.h" 00042 #include "geometry_msgs/PoseWithCovarianceStamped.h" 00043 #include "geometry_msgs/PoseArray.h" 00044 #include "geometry_msgs/Pose.h" 00045 #include "nav_msgs/GetMap.h" 00046 #include "std_srvs/Empty.h" 00047 00048 // For transform support 00049 #include "tf/transform_broadcaster.h" 00050 #include "tf/transform_listener.h" 00051 #include "tf/message_filter.h" 00052 #include "message_filters/subscriber.h" 00053 00054 // Dynamic_reconfigure 00055 #include "dynamic_reconfigure/server.h" 00056 #include "amcl/AMCLConfig.h" 00057 00058 #define NEW_UNIFORM_SAMPLING 1 00059 00060 using namespace amcl; 00061 00062 // Pose hypothesis 00063 typedef struct 00064 { 00065 // Total weight (weights sum to 1) 00066 double weight; 00067 00068 // Mean of pose esimate 00069 pf_vector_t pf_pose_mean; 00070 00071 // Covariance of pose estimate 00072 pf_matrix_t pf_pose_cov; 00073 00074 } amcl_hyp_t; 00075 00076 static double 00077 normalize(double z) 00078 { 00079 return atan2(sin(z),cos(z)); 00080 } 00081 static double 00082 angle_diff(double a, double b) 00083 { 00084 double d1, d2; 00085 a = normalize(a); 00086 b = normalize(b); 00087 d1 = a-b; 00088 d2 = 2*M_PI - fabs(d1); 00089 if(d1 > 0) 00090 d2 *= -1.0; 00091 if(fabs(d1) < fabs(d2)) 00092 return(d1); 00093 else 00094 return(d2); 00095 } 00096 00097 class AmclNode 00098 { 00099 public: 00100 AmclNode(); 00101 ~AmclNode(); 00102 00103 int process(); 00104 00105 private: 00106 tf::TransformBroadcaster* tfb_; 00107 tf::TransformListener* tf_; 00108 00109 bool sent_first_transform_; 00110 00111 tf::Transform latest_tf_; 00112 bool latest_tf_valid_; 00113 00114 // Pose-generating function used to uniformly distribute particles over 00115 // the map 00116 static pf_vector_t uniformPoseGenerator(void* arg); 00117 #if NEW_UNIFORM_SAMPLING 00118 static std::vector<std::pair<int,int> > free_space_indices; 00119 #endif 00120 // Message callbacks 00121 bool globalLocalizationCallback(std_srvs::Empty::Request& req, 00122 std_srvs::Empty::Response& res); 00123 void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan); 00124 void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); 00125 void initialPoseReceivedOld(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); 00126 void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg); 00127 00128 void handleMapMessage(const nav_msgs::OccupancyGrid& msg); 00129 void freeMapDependentMemory(); 00130 map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg ); 00131 void applyInitialPose(); 00132 00133 double getYaw(tf::Pose& t); 00134 00135 //parameter for what odom to use 00136 std::string odom_frame_id_; 00137 //parameter for what base to use 00138 std::string base_frame_id_; 00139 std::string global_frame_id_; 00140 00141 bool use_map_topic_; 00142 bool first_map_only_; 00143 00144 ros::Duration gui_publish_period; 00145 ros::Time save_pose_last_time; 00146 ros::Duration save_pose_period; 00147 00148 geometry_msgs::PoseWithCovarianceStamped last_published_pose; 00149 00150 map_t* map_; 00151 char* mapdata; 00152 int sx, sy; 00153 double resolution; 00154 00155 message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_; 00156 tf::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_; 00157 message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_sub_; 00158 tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>* initial_pose_filter_; 00159 std::vector< AMCLLaser* > lasers_; 00160 std::vector< bool > lasers_update_; 00161 std::map< std::string, int > frame_to_laser_; 00162 00163 // Particle filter 00164 pf_t *pf_; 00165 double pf_err_, pf_z_; 00166 bool pf_init_; 00167 pf_vector_t pf_odom_pose_; 00168 double d_thresh_, a_thresh_; 00169 int resample_interval_; 00170 int resample_count_; 00171 double laser_min_range_; 00172 double laser_max_range_; 00173 00174 AMCLOdom* odom_; 00175 AMCLLaser* laser_; 00176 00177 ros::Duration cloud_pub_interval; 00178 ros::Time last_cloud_pub_time; 00179 00180 void requestMap(); 00181 00182 // Helper to get odometric pose from transform system 00183 bool getOdomPose(tf::Stamped<tf::Pose>& pose, 00184 double& x, double& y, double& yaw, 00185 const ros::Time& t, const std::string& f); 00186 00187 //time for tolerance on the published transform, 00188 //basically defines how long a map->odom transform is good for 00189 ros::Duration transform_tolerance_; 00190 00191 ros::NodeHandle nh_; 00192 ros::NodeHandle private_nh_; 00193 ros::Publisher pose_pub_; 00194 ros::Publisher particlecloud_pub_; 00195 ros::ServiceServer global_loc_srv_; 00196 ros::Subscriber initial_pose_sub_old_; 00197 ros::Subscriber map_sub_; 00198 00199 amcl_hyp_t* initial_pose_hyp_; 00200 bool first_map_received_; 00201 bool first_reconfigure_call_; 00202 00203 boost::recursive_mutex configuration_mutex_; 00204 dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_; 00205 amcl::AMCLConfig default_config_; 00206 00207 int max_beams_, min_particles_, max_particles_; 00208 double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; 00209 double alpha_slow_, alpha_fast_; 00210 double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_; 00211 double laser_likelihood_max_dist_; 00212 odom_model_t odom_model_type_; 00213 double init_pose_[3]; 00214 double init_cov_[3]; 00215 laser_model_t laser_model_type_; 00216 00217 void reconfigureCB(amcl::AMCLConfig &config, uint32_t level); 00218 }; 00219 00220 std::vector<std::pair<int,int> > AmclNode::free_space_indices; 00221 00222 #define USAGE "USAGE: amcl" 00223 00224 int 00225 main(int argc, char** argv) 00226 { 00227 ros::init(argc, argv, "amcl"); 00228 ros::NodeHandle nh; 00229 00230 AmclNode an; 00231 00232 ros::spin(); 00233 00234 // To quote Morgan, Hooray! 00235 return(0); 00236 } 00237 00238 AmclNode::AmclNode() : 00239 sent_first_transform_(false), 00240 latest_tf_valid_(false), 00241 map_(NULL), 00242 pf_(NULL), 00243 resample_count_(0), 00244 odom_(NULL), 00245 laser_(NULL), 00246 private_nh_("~"), 00247 initial_pose_hyp_(NULL), 00248 first_map_received_(false), 00249 first_reconfigure_call_(true) 00250 { 00251 boost::recursive_mutex::scoped_lock l(configuration_mutex_); 00252 00253 // Grab params off the param server 00254 private_nh_.param("use_map_topic", use_map_topic_, false); 00255 private_nh_.param("first_map_only", first_map_only_, false); 00256 00257 double tmp; 00258 private_nh_.param("gui_publish_rate", tmp, -1.0); 00259 gui_publish_period = ros::Duration(1.0/tmp); 00260 private_nh_.param("save_pose_rate", tmp, 0.5); 00261 save_pose_period = ros::Duration(1.0/tmp); 00262 00263 private_nh_.param("laser_min_range", laser_min_range_, -1.0); 00264 private_nh_.param("laser_max_range", laser_max_range_, -1.0); 00265 private_nh_.param("laser_max_beams", max_beams_, 30); 00266 private_nh_.param("min_particles", min_particles_, 100); 00267 private_nh_.param("max_particles", max_particles_, 5000); 00268 private_nh_.param("kld_err", pf_err_, 0.01); 00269 private_nh_.param("kld_z", pf_z_, 0.99); 00270 private_nh_.param("odom_alpha1", alpha1_, 0.2); 00271 private_nh_.param("odom_alpha2", alpha2_, 0.2); 00272 private_nh_.param("odom_alpha3", alpha3_, 0.2); 00273 private_nh_.param("odom_alpha4", alpha4_, 0.2); 00274 private_nh_.param("odom_alpha5", alpha5_, 0.2); 00275 00276 private_nh_.param("laser_z_hit", z_hit_, 0.95); 00277 private_nh_.param("laser_z_short", z_short_, 0.1); 00278 private_nh_.param("laser_z_max", z_max_, 0.05); 00279 private_nh_.param("laser_z_rand", z_rand_, 0.05); 00280 private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2); 00281 private_nh_.param("laser_lambda_short", lambda_short_, 0.1); 00282 private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0); 00283 std::string tmp_model_type; 00284 private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field")); 00285 if(tmp_model_type == "beam") 00286 laser_model_type_ = LASER_MODEL_BEAM; 00287 else if(tmp_model_type == "likelihood_field") 00288 laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; 00289 else 00290 { 00291 ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model", 00292 tmp_model_type.c_str()); 00293 laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; 00294 } 00295 00296 private_nh_.param("odom_model_type", tmp_model_type, std::string("diff")); 00297 if(tmp_model_type == "diff") 00298 odom_model_type_ = ODOM_MODEL_DIFF; 00299 else if(tmp_model_type == "omni") 00300 odom_model_type_ = ODOM_MODEL_OMNI; 00301 else 00302 { 00303 ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model", 00304 tmp_model_type.c_str()); 00305 odom_model_type_ = ODOM_MODEL_DIFF; 00306 } 00307 00308 private_nh_.param("update_min_d", d_thresh_, 0.2); 00309 private_nh_.param("update_min_a", a_thresh_, M_PI/6.0); 00310 private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom")); 00311 private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link")); 00312 private_nh_.param("global_frame_id", global_frame_id_, std::string("map")); 00313 private_nh_.param("resample_interval", resample_interval_, 2); 00314 double tmp_tol; 00315 private_nh_.param("transform_tolerance", tmp_tol, 0.1); 00316 private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001); 00317 private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.1); 00318 00319 transform_tolerance_.fromSec(tmp_tol); 00320 00321 private_nh_.param("initial_pose_x", init_pose_[0], 0.0); 00322 private_nh_.param("initial_pose_y", init_pose_[1], 0.0); 00323 private_nh_.param("initial_pose_a", init_pose_[2], 0.0); 00324 private_nh_.param("initial_cov_xx", init_cov_[0], 0.5 * 0.5); 00325 private_nh_.param("initial_cov_yy", init_cov_[1], 0.5 * 0.5); 00326 private_nh_.param("initial_cov_aa", init_cov_[2], 00327 (M_PI/12.0) * (M_PI/12.0)); 00328 00329 cloud_pub_interval.fromSec(1.0); 00330 tfb_ = new tf::TransformBroadcaster(); 00331 tf_ = new tf::TransformListener(); 00332 00333 pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2); 00334 particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2); 00335 global_loc_srv_ = nh_.advertiseService("global_localization", 00336 &AmclNode::globalLocalizationCallback, 00337 this); 00338 laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, "scan", 100); 00339 laser_scan_filter_ = 00340 new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 00341 *tf_, 00342 odom_frame_id_, 00343 100); 00344 laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, 00345 this, _1)); 00346 initial_pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped>(nh_, "initialpose", 2); 00347 initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, *tf_, global_frame_id_, 2); 00348 initial_pose_filter_->registerCallback(boost::bind(&AmclNode::initialPoseReceived, this, _1)); 00349 00350 initial_pose_sub_old_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceivedOld, this); 00351 00352 if(use_map_topic_) { 00353 map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this); 00354 ROS_INFO("Subscribed to map topic."); 00355 } else { 00356 requestMap(); 00357 } 00358 00359 dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~")); 00360 dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2); 00361 dsrv_->setCallback(cb); 00362 } 00363 00364 void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level) 00365 { 00366 boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); 00367 00368 //we don't want to do anything on the first call 00369 //which corresponds to startup 00370 if(first_reconfigure_call_) 00371 { 00372 first_reconfigure_call_ = false; 00373 default_config_ = config; 00374 return; 00375 } 00376 00377 if(config.restore_defaults) { 00378 config = default_config_; 00379 //avoid looping 00380 config.restore_defaults = false; 00381 } 00382 00383 d_thresh_ = config.update_min_d; 00384 a_thresh_ = config.update_min_a; 00385 00386 resample_interval_ = config.resample_interval; 00387 00388 laser_min_range_ = config.laser_min_range; 00389 laser_max_range_ = config.laser_max_range; 00390 00391 gui_publish_period = ros::Duration(1.0/config.gui_publish_rate); 00392 save_pose_period = ros::Duration(1.0/config.save_pose_rate); 00393 00394 transform_tolerance_.fromSec(config.transform_tolerance); 00395 00396 max_beams_ = config.laser_max_beams; 00397 alpha1_ = config.odom_alpha1; 00398 alpha2_ = config.odom_alpha2; 00399 alpha3_ = config.odom_alpha3; 00400 alpha4_ = config.odom_alpha4; 00401 alpha5_ = config.odom_alpha5; 00402 00403 z_hit_ = config.laser_z_hit; 00404 z_short_ = config.laser_z_short; 00405 z_max_ = config.laser_z_max; 00406 z_rand_ = config.laser_z_rand; 00407 sigma_hit_ = config.laser_sigma_hit; 00408 lambda_short_ = config.laser_lambda_short; 00409 laser_likelihood_max_dist_ = config.laser_likelihood_max_dist; 00410 00411 if(config.laser_model_type == "beam") 00412 laser_model_type_ = LASER_MODEL_BEAM; 00413 else if(config.laser_model_type == "likelihood_field") 00414 laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; 00415 00416 if(config.odom_model_type == "diff") 00417 odom_model_type_ = ODOM_MODEL_DIFF; 00418 else if(config.odom_model_type == "omni") 00419 odom_model_type_ = ODOM_MODEL_OMNI; 00420 00421 if(config.min_particles > config.max_particles) 00422 { 00423 ROS_WARN("You've set min_particles to be less than max particles, this isn't allowed so they'll be set to be equal."); 00424 config.max_particles = config.min_particles; 00425 } 00426 00427 min_particles_ = config.min_particles; 00428 max_particles_ = config.max_particles; 00429 alpha_slow_ = config.recovery_alpha_slow; 00430 alpha_fast_ = config.recovery_alpha_fast; 00431 00432 pf_ = pf_alloc(min_particles_, max_particles_, 00433 alpha_slow_, alpha_fast_, 00434 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, 00435 (void *)map_); 00436 pf_err_ = config.kld_err; 00437 pf_z_ = config.kld_z; 00438 pf_->pop_err = pf_err_; 00439 pf_->pop_z = pf_z_; 00440 00441 // Initialize the filter 00442 pf_vector_t pf_init_pose_mean = pf_vector_zero(); 00443 pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x; 00444 pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y; 00445 pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation); 00446 pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); 00447 pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0]; 00448 pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1]; 00449 pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5]; 00450 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); 00451 pf_init_ = false; 00452 00453 // Instantiate the sensor objects 00454 // Odometry 00455 delete odom_; 00456 odom_ = new AMCLOdom(); 00457 ROS_ASSERT(odom_); 00458 if(odom_model_type_ == ODOM_MODEL_OMNI) 00459 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); 00460 else 00461 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_); 00462 // Laser 00463 delete laser_; 00464 laser_ = new AMCLLaser(max_beams_, map_); 00465 ROS_ASSERT(laser_); 00466 if(laser_model_type_ == LASER_MODEL_BEAM) 00467 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, 00468 sigma_hit_, lambda_short_, 0.0); 00469 else 00470 { 00471 ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); 00472 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, 00473 laser_likelihood_max_dist_); 00474 ROS_INFO("Done initializing likelihood field model."); 00475 } 00476 00477 odom_frame_id_ = config.odom_frame_id; 00478 base_frame_id_ = config.base_frame_id; 00479 global_frame_id_ = config.global_frame_id; 00480 00481 delete laser_scan_filter_; 00482 laser_scan_filter_ = 00483 new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 00484 *tf_, 00485 odom_frame_id_, 00486 100); 00487 laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, 00488 this, _1)); 00489 00490 delete initial_pose_filter_; 00491 initial_pose_filter_ = new tf::MessageFilter<geometry_msgs::PoseWithCovarianceStamped>(*initial_pose_sub_, *tf_, global_frame_id_, 2); 00492 initial_pose_filter_->registerCallback(boost::bind(&AmclNode::initialPoseReceived, this, _1)); 00493 } 00494 00495 void 00496 AmclNode::requestMap() 00497 { 00498 boost::recursive_mutex::scoped_lock ml(configuration_mutex_); 00499 00500 // get map via RPC 00501 nav_msgs::GetMap::Request req; 00502 nav_msgs::GetMap::Response resp; 00503 ROS_INFO("Requesting the map..."); 00504 while(!ros::service::call("static_map", req, resp)) 00505 { 00506 ROS_WARN("Request for map failed; trying again..."); 00507 ros::Duration d(0.5); 00508 d.sleep(); 00509 } 00510 handleMapMessage( resp.map ); 00511 } 00512 00513 void 00514 AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg) 00515 { 00516 if( first_map_only_ && first_map_received_ ) { 00517 return; 00518 } 00519 00520 handleMapMessage( *msg ); 00521 00522 first_map_received_ = true; 00523 } 00524 00525 void 00526 AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) 00527 { 00528 boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); 00529 00530 ROS_INFO("Received a %d X %d map @ %.3f m/pix\n", 00531 msg.info.width, 00532 msg.info.height, 00533 msg.info.resolution); 00534 00535 freeMapDependentMemory(); 00536 00537 map_ = convertMap(msg); 00538 00539 #if NEW_UNIFORM_SAMPLING 00540 // Index of free space 00541 free_space_indices.resize(0); 00542 for(int i = 0; i < map_->size_x; i++) 00543 for(int j = 0; j < map_->size_y; j++) 00544 if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1) 00545 free_space_indices.push_back(std::make_pair(i,j)); 00546 #endif 00547 // Create the particle filter 00548 pf_ = pf_alloc(min_particles_, max_particles_, 00549 alpha_slow_, alpha_fast_, 00550 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, 00551 (void *)map_); 00552 pf_->pop_err = pf_err_; 00553 pf_->pop_z = pf_z_; 00554 00555 // Initialize the filter 00556 pf_vector_t pf_init_pose_mean = pf_vector_zero(); 00557 pf_init_pose_mean.v[0] = init_pose_[0]; 00558 pf_init_pose_mean.v[1] = init_pose_[1]; 00559 pf_init_pose_mean.v[2] = init_pose_[2]; 00560 pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); 00561 pf_init_pose_cov.m[0][0] = init_cov_[0]; 00562 pf_init_pose_cov.m[1][1] = init_cov_[1]; 00563 pf_init_pose_cov.m[2][2] = init_cov_[2]; 00564 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov); 00565 pf_init_ = false; 00566 00567 // Instantiate the sensor objects 00568 // Odometry 00569 delete odom_; 00570 odom_ = new AMCLOdom(); 00571 ROS_ASSERT(odom_); 00572 if(odom_model_type_ == ODOM_MODEL_OMNI) 00573 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_); 00574 else 00575 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_); 00576 // Laser 00577 delete laser_; 00578 laser_ = new AMCLLaser(max_beams_, map_); 00579 ROS_ASSERT(laser_); 00580 if(laser_model_type_ == LASER_MODEL_BEAM) 00581 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, 00582 sigma_hit_, lambda_short_, 0.0); 00583 else 00584 { 00585 ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); 00586 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, 00587 laser_likelihood_max_dist_); 00588 ROS_INFO("Done initializing likelihood field model."); 00589 } 00590 00591 // In case the initial pose message arrived before the first map, 00592 // try to apply the initial pose now that the map has arrived. 00593 applyInitialPose(); 00594 00595 } 00596 00597 void 00598 AmclNode::freeMapDependentMemory() 00599 { 00600 if( map_ != NULL ) { 00601 map_free( map_ ); 00602 map_ = NULL; 00603 } 00604 if( pf_ != NULL ) { 00605 pf_free( pf_ ); 00606 pf_ = NULL; 00607 } 00608 delete odom_; 00609 odom_ = NULL; 00610 delete laser_; 00611 laser_ = NULL; 00612 } 00613 00618 map_t* 00619 AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg ) 00620 { 00621 map_t* map = map_alloc(); 00622 ROS_ASSERT(map); 00623 00624 map->size_x = map_msg.info.width; 00625 map->size_y = map_msg.info.height; 00626 map->scale = map_msg.info.resolution; 00627 map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale; 00628 map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale; 00629 // Convert to player format 00630 map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y); 00631 ROS_ASSERT(map->cells); 00632 for(int i=0;i<map->size_x * map->size_y;i++) 00633 { 00634 if(map_msg.data[i] == 0) 00635 map->cells[i].occ_state = -1; 00636 else if(map_msg.data[i] == 100) 00637 map->cells[i].occ_state = +1; 00638 else 00639 map->cells[i].occ_state = 0; 00640 } 00641 00642 return map; 00643 } 00644 00645 AmclNode::~AmclNode() 00646 { 00647 delete dsrv_; 00648 freeMapDependentMemory(); 00649 delete laser_scan_filter_; 00650 delete laser_scan_sub_; 00651 delete initial_pose_filter_; 00652 delete initial_pose_sub_; 00653 delete tfb_; 00654 delete tf_; 00655 // TODO: delete everything allocated in constructor 00656 } 00657 00658 bool 00659 AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose, 00660 double& x, double& y, double& yaw, 00661 const ros::Time& t, const std::string& f) 00662 { 00663 // Get the robot's pose 00664 tf::Stamped<tf::Pose> ident (btTransform(tf::createIdentityQuaternion(), 00665 btVector3(0,0,0)), t, f); 00666 try 00667 { 00668 this->tf_->transformPose(odom_frame_id_, ident, odom_pose); 00669 } 00670 catch(tf::TransformException e) 00671 { 00672 ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); 00673 return false; 00674 } 00675 x = odom_pose.getOrigin().x(); 00676 y = odom_pose.getOrigin().y(); 00677 double pitch,roll; 00678 odom_pose.getBasis().getEulerYPR(yaw, pitch, roll); 00679 00680 return true; 00681 } 00682 00683 00684 pf_vector_t 00685 AmclNode::uniformPoseGenerator(void* arg) 00686 { 00687 map_t* map = (map_t*)arg; 00688 #if NEW_UNIFORM_SAMPLING 00689 unsigned int rand_index = drand48() * free_space_indices.size(); 00690 std::pair<int,int> free_point = free_space_indices[rand_index]; 00691 pf_vector_t p; 00692 p.v[0] = MAP_WXGX(map, free_point.first); 00693 p.v[1] = MAP_WYGY(map, free_point.second); 00694 p.v[2] = drand48() * 2 * M_PI - M_PI; 00695 #else 00696 double min_x, max_x, min_y, max_y; 00697 00698 min_x = (map->size_x * map->scale)/2.0 - map->origin_x; 00699 max_x = (map->size_x * map->scale)/2.0 + map->origin_x; 00700 min_y = (map->size_y * map->scale)/2.0 - map->origin_y; 00701 max_y = (map->size_y * map->scale)/2.0 + map->origin_y; 00702 00703 pf_vector_t p; 00704 00705 ROS_DEBUG("Generating new uniform sample"); 00706 for(;;) 00707 { 00708 p.v[0] = min_x + drand48() * (max_x - min_x); 00709 p.v[1] = min_y + drand48() * (max_y - min_y); 00710 p.v[2] = drand48() * 2 * M_PI - M_PI; 00711 // Check that it's a free cell 00712 int i,j; 00713 i = MAP_GXWX(map, p.v[0]); 00714 j = MAP_GYWY(map, p.v[1]); 00715 if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1)) 00716 break; 00717 } 00718 #endif 00719 return p; 00720 } 00721 00722 bool 00723 AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req, 00724 std_srvs::Empty::Response& res) 00725 { 00726 if( map_ == NULL ) { 00727 return true; 00728 } 00729 boost::recursive_mutex::scoped_lock gl(configuration_mutex_); 00730 ROS_INFO("Initializing with uniform distribution"); 00731 pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator, 00732 (void *)map_); 00733 ROS_INFO("Global initialisation done!"); 00734 pf_init_ = false; 00735 return true; 00736 } 00737 00738 void 00739 AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) 00740 { 00741 if( map_ == NULL ) { 00742 return; 00743 } 00744 boost::recursive_mutex::scoped_lock lr(configuration_mutex_); 00745 int laser_index = -1; 00746 00747 // Do we have the base->base_laser Tx yet? 00748 if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end()) 00749 { 00750 ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str()); 00751 lasers_.push_back(new AMCLLaser(*laser_)); 00752 lasers_update_.push_back(true); 00753 laser_index = frame_to_laser_.size(); 00754 00755 tf::Stamped<tf::Pose> ident (btTransform(tf::createIdentityQuaternion(), 00756 btVector3(0,0,0)), 00757 ros::Time(), laser_scan->header.frame_id); 00758 tf::Stamped<tf::Pose> laser_pose; 00759 try 00760 { 00761 this->tf_->transformPose(base_frame_id_, ident, laser_pose); 00762 } 00763 catch(tf::TransformException& e) 00764 { 00765 ROS_ERROR("Couldn't transform from %s to %s, " 00766 "even though the message notifier is in use", 00767 laser_scan->header.frame_id.c_str(), 00768 base_frame_id_.c_str()); 00769 return; 00770 } 00771 00772 pf_vector_t laser_pose_v; 00773 laser_pose_v.v[0] = laser_pose.getOrigin().x(); 00774 laser_pose_v.v[1] = laser_pose.getOrigin().y(); 00775 // laser mounting angle gets computed later -> set to 0 here! 00776 laser_pose_v.v[2] = 0; 00777 lasers_[laser_index]->SetLaserPose(laser_pose_v); 00778 ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f", 00779 laser_pose_v.v[0], 00780 laser_pose_v.v[1], 00781 laser_pose_v.v[2]); 00782 00783 frame_to_laser_[laser_scan->header.frame_id] = laser_index; 00784 } else { 00785 // we have the laser pose, retrieve laser index 00786 laser_index = frame_to_laser_[laser_scan->header.frame_id]; 00787 } 00788 00789 // Where was the robot when this scan was taken? 00790 tf::Stamped<tf::Pose> odom_pose; 00791 pf_vector_t pose; 00792 if(!getOdomPose(odom_pose, pose.v[0], pose.v[1], pose.v[2], 00793 laser_scan->header.stamp, base_frame_id_)) 00794 { 00795 ROS_ERROR("Couldn't determine robot's pose associated with laser scan"); 00796 return; 00797 } 00798 00799 00800 pf_vector_t delta = pf_vector_zero(); 00801 00802 if(pf_init_) 00803 { 00804 // Compute change in pose 00805 //delta = pf_vector_coord_sub(pose, pf_odom_pose_); 00806 delta.v[0] = pose.v[0] - pf_odom_pose_.v[0]; 00807 delta.v[1] = pose.v[1] - pf_odom_pose_.v[1]; 00808 delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]); 00809 00810 // See if we should update the filter 00811 bool update = fabs(delta.v[0]) > d_thresh_ || 00812 fabs(delta.v[1]) > d_thresh_ || 00813 fabs(delta.v[2]) > a_thresh_; 00814 00815 // Set the laser update flags 00816 if(update) 00817 for(unsigned int i=0; i < lasers_update_.size(); i++) 00818 lasers_update_[i] = true; 00819 } 00820 00821 bool force_publication = false; 00822 if(!pf_init_) 00823 { 00824 // Pose at last filter update 00825 pf_odom_pose_ = pose; 00826 00827 // Filter is now initialized 00828 pf_init_ = true; 00829 00830 // Should update sensor data 00831 for(unsigned int i=0; i < lasers_update_.size(); i++) 00832 lasers_update_[i] = true; 00833 00834 force_publication = true; 00835 00836 resample_count_ = 0; 00837 } 00838 // If the robot has moved, update the filter 00839 else if(pf_init_ && lasers_update_[laser_index]) 00840 { 00841 //printf("pose\n"); 00842 //pf_vector_fprintf(pose, stdout, "%.3f"); 00843 00844 AMCLOdomData odata; 00845 odata.pose = pose; 00846 // HACK 00847 // Modify the delta in the action data so the filter gets 00848 // updated correctly 00849 odata.delta = delta; 00850 00851 // Use the action data to update the filter 00852 odom_->UpdateAction(pf_, (AMCLSensorData*)&odata); 00853 00854 // Pose at last filter update 00855 //this->pf_odom_pose = pose; 00856 } 00857 00858 bool resampled = false; 00859 // If the robot has moved, update the filter 00860 if(lasers_update_[laser_index]) 00861 { 00862 AMCLLaserData ldata; 00863 ldata.sensor = lasers_[laser_index]; 00864 ldata.range_count = laser_scan->ranges.size(); 00865 00866 // To account for lasers that are mounted upside-down, we determine the 00867 // min, max, and increment angles of the laser in the base frame. 00868 // 00869 // Construct min and max angles of laser, in the base_link frame. 00870 tf::Quaternion q; 00871 q.setRPY(0.0, 0.0, laser_scan->angle_min); 00872 tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp, 00873 laser_scan->header.frame_id); 00874 q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); 00875 tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp, 00876 laser_scan->header.frame_id); 00877 try 00878 { 00879 tf_->transformQuaternion(base_frame_id_, min_q, min_q); 00880 tf_->transformQuaternion(base_frame_id_, inc_q, inc_q); 00881 } 00882 catch(tf::TransformException& e) 00883 { 00884 ROS_WARN("Unable to transform min/max laser angles into base frame: %s", 00885 e.what()); 00886 return; 00887 } 00888 00889 double angle_min = tf::getYaw(min_q); 00890 double angle_increment = tf::getYaw(inc_q) - angle_min; 00891 00892 // wrapping angle to [-pi .. pi] 00893 angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI; 00894 00895 ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); 00896 00897 // Apply range min/max thresholds, if the user supplied them 00898 if(laser_max_range_ > 0.0) 00899 ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_); 00900 else 00901 ldata.range_max = laser_scan->range_max; 00902 double range_min; 00903 if(laser_min_range_ > 0.0) 00904 range_min = std::max(laser_scan->range_min, (float)laser_min_range_); 00905 else 00906 range_min = laser_scan->range_min; 00907 // The AMCLLaserData destructor will free this memory 00908 ldata.ranges = new double[ldata.range_count][2]; 00909 ROS_ASSERT(ldata.ranges); 00910 for(int i=0;i<ldata.range_count;i++) 00911 { 00912 // amcl doesn't (yet) have a concept of min range. So we'll map short 00913 // readings to max range. 00914 if(laser_scan->ranges[i] <= range_min) 00915 ldata.ranges[i][0] = ldata.range_max; 00916 else 00917 ldata.ranges[i][0] = laser_scan->ranges[i]; 00918 // Compute bearing 00919 ldata.ranges[i][1] = angle_min + 00920 (i * angle_increment); 00921 } 00922 00923 lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata); 00924 00925 lasers_update_[laser_index] = false; 00926 00927 pf_odom_pose_ = pose; 00928 00929 // Resample the particles 00930 if(!(++resample_count_ % resample_interval_)) 00931 { 00932 pf_update_resample(pf_); 00933 resampled = true; 00934 } 00935 00936 pf_sample_set_t* set = pf_->sets + pf_->current_set; 00937 ROS_DEBUG("Num samples: %d\n", set->sample_count); 00938 00939 // Publish the resulting cloud 00940 // TODO: set maximum rate for publishing 00941 geometry_msgs::PoseArray cloud_msg; 00942 cloud_msg.header.stamp = ros::Time::now(); 00943 cloud_msg.header.frame_id = global_frame_id_; 00944 cloud_msg.poses.resize(set->sample_count); 00945 for(int i=0;i<set->sample_count;i++) 00946 { 00947 tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]), 00948 btVector3(set->samples[i].pose.v[0], 00949 set->samples[i].pose.v[1], 0)), 00950 cloud_msg.poses[i]); 00951 00952 } 00953 00954 particlecloud_pub_.publish(cloud_msg); 00955 } 00956 00957 if(resampled || force_publication) 00958 { 00959 // Read out the current hypotheses 00960 double max_weight = 0.0; 00961 int max_weight_hyp = -1; 00962 std::vector<amcl_hyp_t> hyps; 00963 hyps.resize(pf_->sets[pf_->current_set].cluster_count); 00964 for(int hyp_count = 0; 00965 hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++) 00966 { 00967 double weight; 00968 pf_vector_t pose_mean; 00969 pf_matrix_t pose_cov; 00970 if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov)) 00971 { 00972 ROS_ERROR("Couldn't get stats on cluster %d", hyp_count); 00973 break; 00974 } 00975 00976 hyps[hyp_count].weight = weight; 00977 hyps[hyp_count].pf_pose_mean = pose_mean; 00978 hyps[hyp_count].pf_pose_cov = pose_cov; 00979 00980 if(hyps[hyp_count].weight > max_weight) 00981 { 00982 max_weight = hyps[hyp_count].weight; 00983 max_weight_hyp = hyp_count; 00984 } 00985 } 00986 00987 if(max_weight > 0.0) 00988 { 00989 ROS_DEBUG("Max weight pose: %.3f %.3f %.3f", 00990 hyps[max_weight_hyp].pf_pose_mean.v[0], 00991 hyps[max_weight_hyp].pf_pose_mean.v[1], 00992 hyps[max_weight_hyp].pf_pose_mean.v[2]); 00993 00994 /* 00995 puts(""); 00996 pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f"); 00997 puts(""); 00998 */ 00999 01000 geometry_msgs::PoseWithCovarianceStamped p; 01001 // Fill in the header 01002 p.header.frame_id = global_frame_id_; 01003 p.header.stamp = laser_scan->header.stamp; 01004 // Copy in the pose 01005 p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; 01006 p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; 01007 tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), 01008 p.pose.pose.orientation); 01009 // Copy in the covariance, converting from 3-D to 6-D 01010 pf_sample_set_t* set = pf_->sets + pf_->current_set; 01011 for(int i=0; i<2; i++) 01012 { 01013 for(int j=0; j<2; j++) 01014 { 01015 // Report the overall filter covariance, rather than the 01016 // covariance for the highest-weight cluster 01017 //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; 01018 p.pose.covariance[6*i+j] = set->cov.m[i][j]; 01019 } 01020 } 01021 // Report the overall filter covariance, rather than the 01022 // covariance for the highest-weight cluster 01023 //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2]; 01024 p.pose.covariance[6*5+5] = set->cov.m[2][2]; 01025 01026 /* 01027 printf("cov:\n"); 01028 for(int i=0; i<6; i++) 01029 { 01030 for(int j=0; j<6; j++) 01031 printf("%6.3f ", p.covariance[6*i+j]); 01032 puts(""); 01033 } 01034 */ 01035 01036 pose_pub_.publish(p); 01037 last_published_pose = p; 01038 01039 ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", 01040 hyps[max_weight_hyp].pf_pose_mean.v[0], 01041 hyps[max_weight_hyp].pf_pose_mean.v[1], 01042 hyps[max_weight_hyp].pf_pose_mean.v[2]); 01043 01044 // subtracting base to odom from map to base and send map to odom instead 01045 tf::Stamped<tf::Pose> odom_to_map; 01046 try 01047 { 01048 tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), 01049 tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], 01050 hyps[max_weight_hyp].pf_pose_mean.v[1], 01051 0.0)); 01052 tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), 01053 laser_scan->header.stamp, 01054 base_frame_id_); 01055 this->tf_->transformPose(odom_frame_id_, 01056 tmp_tf_stamped, 01057 odom_to_map); 01058 } 01059 catch(tf::TransformException) 01060 { 01061 ROS_DEBUG("Failed to subtract base to odom transform"); 01062 return; 01063 } 01064 01065 latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), 01066 tf::Point(odom_to_map.getOrigin())); 01067 latest_tf_valid_ = true; 01068 01069 // We want to send a transform that is good up until a 01070 // tolerance time so that odom can be used 01071 ros::Time transform_expiration = (laser_scan->header.stamp + 01072 transform_tolerance_); 01073 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), 01074 transform_expiration, 01075 global_frame_id_, odom_frame_id_); 01076 this->tfb_->sendTransform(tmp_tf_stamped); 01077 sent_first_transform_ = true; 01078 } 01079 else 01080 { 01081 ROS_ERROR("No pose!"); 01082 } 01083 } 01084 else if(latest_tf_valid_) 01085 { 01086 // Nothing changed, so we'll just republish the last transform, to keep 01087 // everybody happy. 01088 ros::Time transform_expiration = (laser_scan->header.stamp + 01089 transform_tolerance_); 01090 tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), 01091 transform_expiration, 01092 global_frame_id_, odom_frame_id_); 01093 this->tfb_->sendTransform(tmp_tf_stamped); 01094 01095 01096 // Is it time to save our last pose to the param server 01097 ros::Time now = ros::Time::now(); 01098 if((save_pose_period.toSec() > 0.0) && 01099 (now - save_pose_last_time) >= save_pose_period) 01100 { 01101 // We need to apply the last transform to the latest odom pose to get 01102 // the latest map pose to store. We'll take the covariance from 01103 // last_published_pose. 01104 tf::Pose map_pose = latest_tf_.inverse() * odom_pose; 01105 double yaw,pitch,roll; 01106 map_pose.getBasis().getEulerYPR(yaw, pitch, roll); 01107 01108 private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x()); 01109 private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y()); 01110 private_nh_.setParam("initial_pose_a", yaw); 01111 private_nh_.setParam("initial_cov_xx", 01112 last_published_pose.pose.covariance[6*0+0]); 01113 private_nh_.setParam("initial_cov_yy", 01114 last_published_pose.pose.covariance[6*1+1]); 01115 private_nh_.setParam("initial_cov_aa", 01116 last_published_pose.pose.covariance[6*5+5]); 01117 save_pose_last_time = now; 01118 } 01119 } 01120 01121 } 01122 01123 double 01124 AmclNode::getYaw(tf::Pose& t) 01125 { 01126 double yaw, pitch, roll; 01127 btMatrix3x3 mat = t.getBasis(); 01128 mat.getEulerYPR(yaw,pitch,roll); 01129 return yaw; 01130 } 01131 01132 void 01133 AmclNode::initialPoseReceivedOld(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) 01134 { 01135 // Support old behavior, where null frame ids were accepted. 01136 if(msg->header.frame_id == "") 01137 { 01138 ROS_WARN("Received initialpose message with header.frame_id == "". This behavior is deprecated; you should always set the frame_id"); 01139 initialPoseReceived(msg); 01140 } 01141 } 01142 01143 void 01144 AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) 01145 { 01146 boost::recursive_mutex::scoped_lock prl(configuration_mutex_); 01147 // In case the client sent us a pose estimate in the past, integrate the 01148 // intervening odometric change. 01149 tf::StampedTransform tx_odom; 01150 try 01151 { 01152 tf_->lookupTransform(base_frame_id_, ros::Time::now(), 01153 base_frame_id_, msg->header.stamp, 01154 global_frame_id_, tx_odom); 01155 } 01156 catch(tf::TransformException e) 01157 { 01158 // If we've never sent a transform, then this is normal, because the 01159 // global_frame_id_ frame doesn't exist. We only care about in-time 01160 // transformation for on-the-move pose-setting, so ignoring this 01161 // startup condition doesn't really cost us anything. 01162 if(sent_first_transform_) 01163 ROS_WARN("Failed to transform initial pose in time (%s)", e.what()); 01164 tx_odom.setIdentity(); 01165 } 01166 01167 tf::Pose pose_old, pose_new; 01168 tf::poseMsgToTF(msg->pose.pose, pose_old); 01169 pose_new = tx_odom.inverse() * pose_old; 01170 01171 ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f", 01172 ros::Time::now().toSec(), 01173 pose_new.getOrigin().x(), 01174 pose_new.getOrigin().y(), 01175 getYaw(pose_new)); 01176 // Re-initialize the filter 01177 pf_vector_t pf_init_pose_mean = pf_vector_zero(); 01178 pf_init_pose_mean.v[0] = pose_new.getOrigin().x(); 01179 pf_init_pose_mean.v[1] = pose_new.getOrigin().y(); 01180 pf_init_pose_mean.v[2] = getYaw(pose_new); 01181 pf_matrix_t pf_init_pose_cov = pf_matrix_zero(); 01182 // Copy in the covariance, converting from 6-D to 3-D 01183 for(int i=0; i<2; i++) 01184 { 01185 for(int j=0; j<2; j++) 01186 { 01187 pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j]; 01188 } 01189 } 01190 pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5]; 01191 01192 delete initial_pose_hyp_; 01193 initial_pose_hyp_ = new amcl_hyp_t(); 01194 initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; 01195 initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; 01196 applyInitialPose(); 01197 } 01198 01204 void 01205 AmclNode::applyInitialPose() 01206 { 01207 boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); 01208 if( initial_pose_hyp_ != NULL && map_ != NULL ) { 01209 pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov); 01210 pf_init_ = false; 01211 01212 delete initial_pose_hyp_; 01213 initial_pose_hyp_ = NULL; 01214 } 01215 }