Augmented_MCL： 在機(jī)器人遭到綁架的時(shí)候，它會(huì)在發(fā)現(xiàn)粒子們的平均分?jǐn)?shù)突然降低了，這意味著正確的粒子在某次迭代中被拋棄了，此時(shí)會(huì)隨機(jī)的全局注入粒子（injection of random particles）。
intmain(int argc,char** argv){ros::init(argc, argv,"amcl");ros::NodeHandle nh;// Override default sigint handlersignal(SIGINT, sigintHandler);// Make our node available to sigintHandleramcl_node_ptr.reset(newAmclNode());if(argc ==1){// run using ROS inputros::spin();}elseif((argc >=3)&&(std::string(argv[1])=="--run-from-bag")){if(argc ==3){amcl_node_ptr->runFromBag(argv[2]);}elseif((argc ==4)&&(std::string(argv[3])=="--global-localization")){amcl_node_ptr->runFromBag(argv[2],true);}}// Without this, our boost locks are not shut down nicelyamcl_node_ptr.reset();// To quote Morgan, Hooray!return(0);}
AmclNode::AmclNode():......private_nh_.param("laser_min_range", laser_min_range_,-1.0);private_nh_.param("laser_max_range", laser_max_range_,-1.0);private_nh_.param("laser_max_beams", max_beams_,30);private_nh_.param("min_particles", min_particles_,100);private_nh_.param("max_particles", max_particles_,5000);......base_frame_id_ =stripSlash(base_frame_id_);global_frame_id_ =stripSlash(global_frame_id_);//init the poseupdatePoseFromServer();cloud_pub_interval.fromSec(1.0);//tfb is the tf Broadcastertfb_.reset(new tf2_ros::TransformBroadcaster());// tf_ use to buffer the scan msg tf_.reset(new tf2_ros::Buffer());//tfl is the tf Listener tfl_.reset(new tf2_ros::TransformListener(*tf_));//publish the topic "amcl_pose"pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",2,true);//publish the topic "particlecloud"particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud",2,true);//create the service of "global_localization". globalLocalizationCallback is init the pfsglobal_loc_srv_ = nh_.advertiseService("global_localization",&AmclNode::globalLocalizationCallback,this);//create the service of "request_nomotion_update,"nomotionUpdateCallback force samples updates without motion reveivednomotion_update_srv_= nh_.advertiseService("request_nomotion_update",&AmclNode::nomotionUpdateCallback,this);set_map_srv_= nh_.advertiseService("set_map",&AmclNode::setMapCallback,this);//subscribe scan topic,MessageFilter used to filter and buffer the scan message util scan could be tramform to odom_framelaser_scan_sub_ =new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_,100);laser_scan_filter_ =new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_,*tf_,odom_frame_id_,100,nh_);//when received scan msg, laserReceived be calledlaser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));//subscibe topic "initialpose",initialPoseReceived used to init pose that service received,if this not be call, then use map origin to init pose by globalLocalizationCallbackinitial_pose_sub_ = nh_.subscribe("initialpose",2,&AmclNode::initialPoseReceived,this);//subscribe topic map, get the map, mapReceived will be called//handleMapMessage is used to deal with map message//when map msg received, the pf, odom and laser be created and initedif(use_map_topic_){map_sub_ = nh_.subscribe("map",1,&AmclNode::mapReceived,this);ROS_INFO("Subscribed to map topic.");}else{......}
void AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg){if( first_map_only_ && first_map_received_ ){return;}handleMapMessage(*msg );first_map_received_ =true;}//free_space_indices store the free cell index//alloc the pfs//new the AMCL odom and set the model of robot//new the AMCL_laser, this stores map ptr,max num of particles likelyhood typevoid
AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg){// if the config is being writen, stock and waitboost::recursive_mutex::scoped_lock cfl(configuration_mutex_);......map_ =convertMap(msg);#if NEW_UNIFORM_SAMPLING// Index of free spacefree_space_indices.resize(0);for(int i =0; i < map_->size_x; i++)for(int j =0; j < map_->size_y; j++)if(map_->cells[MAP_INDEX(map_,i,j)].occ_state ==-1)free_space_indices.push_back(std::make_pair(i,j));#endif// Create the particle filterpf_ =pf_alloc(min_particles_, max_particles_,alpha_slow_, alpha_fast_,(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,(void*)map_);// Instantiate the sensor objects// Odometrydelete odom_;odom_ =newAMCLOdom();ROS_ASSERT(odom_);odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );// Laserdelete laser_;laser_ =newAMCLLaser(max_beams_, map_);ROS_ASSERT(laser_);if(laser_model_type_ == LASER_MODEL_BEAM)laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,sigma_hit_, lambda_short_,0.0);elseif(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){......}}
// if the laserpose in base frame is not inited,then get the laser pose in base frame// get the odom frame// determine whether update pf or not// resamping after updatevoid AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan){// laser_scan_frame_id is the name of lase_scan_frame in string// get the lase_scan_frame name in stringstd::string laser_scan_frame_id =stripSlash(laser_scan->header.frame_id);......//laser_pose is the laser pose in base frame,this is a constant//laser_pose is the transform form base_laser to basegeometry_msgs::PoseStamped laser_pose;try{this->tf_->transform(ident, laser_pose, base_frame_id_);}......// Where was the robot when this scan was taken?// get the robot pose in odom framepf_vector_t pose;if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],laser_scan->header.stamp, base_frame_id_))......// See if we should update the filterbool update =fabs(delta.v[0])> d_thresh_ ||fabs(delta.v[1])> d_thresh_ ||fabs(delta.v[2])> a_thresh_;//m_force_update used to temporarily let amcl update samples even when no motion occursupdate = update || m_force_update;......// Use the action data to update the filter//利用運(yùn)動(dòng)學(xué)模型更新粒子位姿odom_->UpdateAction(pf_,(AMCLSensorData*)&odata);......// If the robot has moved, update the filterif(lasers_update_[laser_index])......// UpdateSensor use the observe mode to compute the sample's weight//使用觀測模型來更新粒子權(quán)重lasers_[laser_index]->UpdateSensor(pf_,(AMCLSensorData*)&ldata);......// Resample the particlesif(!(++resample_count_ % resample_interval_)){pf_update_resample(pf_);resampled =true;}pf_sample_set_t* set = pf_->sets + pf_->current_set;
//單個(gè)粒子：代表位姿，權(quán)重// Information for a single sampletypedefstruct{// Pose represented by this samplepf_vector_t pose;// Weight for this posedouble weight;} pf_sample_t;//粒子簇結(jié)構(gòu)，一個(gè)位姿節(jié)點(diǎn)的粒子簇// Information for a cluster of samplestypedefstruct{// Number of samplesint count;// Total weight of samples in this clusterdouble weight;// Cluster statisticspf_vector_t mean;pf_matrix_t cov// Workspacedouble m[4], c[2][2];} pf_cluster_t;// 粒子集，kdtree存儲，包含多個(gè)粒子簇，多個(gè)位姿節(jié)點(diǎn)粒子簇// Information for a set of samplestypedefstruct _pf_sample_set_t
{// The samplesint sample_count;pf_sample_t *samples;// A kdtree encoding the histogrampf_kdtree_t *kdtree;// Clustersint cluster_count, cluster_max_count;pf_cluster_t *clusters;// Filter statisticspf_vector_t mean;pf_matrix_t cov;int converged;double n_effective;} pf_sample_set_t;// 粒子濾波器結(jié)構(gòu)體，包含了配置信息typedefstruct _pf_t
{// This min and max number of samplesint min_samples, max_samples;// Population size parametersdouble pop_err, pop_z;// The sample sets. We keep two sets and use [current_set]// to identify the active set.int current_set;pf_sample_set_t sets[2];// Running averages, slow and fast, of likelihooddouble w_slow, w_fast;// Decay rates for running averagesdouble alpha_slow, alpha_fast;// Function used to draw random pose samplespf_init_model_fn_t random_pose_fn;void*random_pose_data;double dist_threshold;//distance threshold in each axis over which the pf is considered to not be convergedint converged;// boolean parameter to enamble/diable selective resamplingint selective_resampling;} pf_t;
