amcl源码解读
int
main(int argc, char** argv)
{
ros::init(argc, argv, "amcl");
ros::NodeHandle nh;
signal(SIGINT, sigintHandler);
amcl_node_ptr.reset(new AmclNode());
if (argc == 1)
{
ros::spin();
}
else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
{
amcl_node_ptr->runFromBag(argv[2]);
}
amcl_node_ptr.reset();
return(0);
}首先是main函数，第一步常规操作初始化了amcl的ros节点，并实例化。第二步使用signal函数：第一个参数为信号功能参数，第二个参数为自定义函数，选用SIGINT表示信号中断即用户输入ctrl+c，之后信号被捕获，进入自定义的信号处理函数中
void sigintHandler(int sig)
{
// Save latest pose as we're shutting down.
amcl_node_ptr->savePoseToServer();
ros::shutdown();
}这个函数中使用到了智能指针，他的声明在这
boost::shared_ptr amcl_node_ptr; boost::shared_ptr是智能指针;
amcl_node_ptr.reset(new AmclNode());shared_ptr可以通过reset方法初始化对象，此时利用AmclNode类构造函数对指针进行初始化。首先给对象分配内存，地址为x，然后调用类构造函数y，将类构造函数的地址y写入x中。接下来就是amcl的构造函数：
AmclNode::AmclNode()
{
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
double tmp;
std::string tmp_model_type;
transform_tolerance_.fromSec(tmp_tol);
{
double bag_scan_period;
private_nh_.param("bag_scan_period", bag_scan_period, -1.0);
bag_scan_period_.fromSec(bag_scan_period);
}
updatePoseFromServer();
cloud_pub_interval.fromSec(1.0);
tfb_.reset(new tf2_ros::TransformBroadcaster());
tf_.reset(new tf2_ros::Buffer());
tfl_.reset(new tf2_ros::TransformListener(*tf_));
pose_pub_ = nh_.advertise("amcl_pose", 2, true);
particlecloud_pub_ = nh_.advertise("particlecloud", 2, true);
global_loc_srv_ = nh_.advertiseService("global_localization",
&AmclNode::globalLocalizationCallback,
this);
nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);
laser_scan_sub_ = new message_filters::Subscriber(nh_, scan_topic_, 100);
laser_scan_filter_ =
new tf2_ros::MessageFilter(*laser_scan_sub_,
*tf_,
odom_frame_id_,
100,
nh_);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
this, _1));
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
if(use_map_topic_) {
map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
ROS_INFO("Subscribed to map topic.");
} else {
requestMap();
}
m_force_update = false;
dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~"));
dynamic_reconfigure::Server::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
// 15s timer to warn on lack of receipt of laser scans, #5209
laser_check_interval_ = ros::Duration(15.0);
check_laser_timer_ = nh_.createTimer(laser_check_interval_,
boost::bind(&AmclNode::checkLaserReceived, this, _1));
diagnosic_updater_.setHardwareID("None");
diagnosic_updater_.add("Standard deviation", this, &AmclNode::standardDeviationDiagnostics);
} amcl的构造函数，一大部分用于装载参数，这部分由于篇幅原因被我无情删除。解读剩下的部分：首先是这句代码定义递归互斥量
boost::recursive_mutex::scoped_lock l(configuration_mutex_);这是boost库中的线程区域锁，锁的初始化放在amcl构造函数中，锁的释放放在析构函数中，为了防止多线程访问共享资源而产生的对数据篡改。
updatePoseFromServer();装载初始位姿。接下来三行是智能指针，并且三个对象分别是发布tf的指针，接收tf的指针指向buffer的指针，都进行构造函数的初始化。
std::shared_ptr tfb_;
std::shared_ptr tfl_;
std::shared_ptr tf_;
tfb_.reset(new tf2_ros::TransformBroadcaster());
tf_.reset(new tf2_ros::Buffer());
tfl_.reset(new tf2_ros::TransformListener(*tf_)); 之后的部分则是，定义发布amcl_pose、particlecloud话题，定义gloabl_localization、request_nomotion_update、set_map服务端，定义话题订阅端，订阅initialpose、map、scan。其中有这么一句消息过滤器，作用是同步tf和scan信号:
laser_scan_sub_ = new message_filters::Subscriber(nh_, scan_topic_, 100);
laser_scan_filter_ =
new tf2_ros::MessageFilter(*laser_scan_sub_,
*tf_,
odom_frame_id_,
100,
nh_);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
this, _1)); 具体函数功能在http://docs.ros.org/api/tf2_ros/html/c++/classtf2__ros_1_1MessageFilter.html#aaad9d03a10d89188c0270e5440d19713这里。下面的代码是订阅map，默认的use_map_topic是false，所以默认调用的是requetMap函数。
if(use_map_topic_) {
map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
ROS_INFO("Subscribed to map topic.");
} else {
requestMap();
}来到requestMap函数：首先依旧是线程锁，之后是一个while函数阻塞等待地图，等到地图后跳出while循环，执行handleMapMessage()函数，地图信息装载在resp对象中。
void AmclNode::requestMap()
{
boost::recursive_mutex::scoped_lock ml(configuration_mutex_);
// get map via RPC
nav_msgs::GetMap::Request req;
nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
ROS_WARN("Request for map failed; trying again...");
ros::Duration d(0.5);
d.sleep();
}
handleMapMessage( resp.map );
}接下来是handleMapMessage函数中：下面的函数用于清空map_,pf_,odom_,laser_。
std::vector< AMCLLaser* > lasers_;
std::vector< bool > lasers_update_;
std::map< std::string, int > frame_to_laser_;
freeMapDependentMemory();
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();清空完所有信息，保存接收到的地图信息，并进行填充：
map_ = convertMap(msg);进入convertMap函数中：这里导入了map.yaml文件里的参数，并把地图的数据转为地图每个点的状态都抱存在map->cell[].occ_state中（map_是个指针，指向的cell也是指针，通过数组化转换为内容，并对数组内的成员进行赋值。
map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
map_t* map = map_alloc();
ROS_ASSERT(map);
map->size_x = map_msg.info.width;
map->size_y = map_msg.info.height;
map->scale = map_msg.info.resolution;
map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
ROS_ASSERT(map->cells);
for(int i=0;isize_x * map->size_y;i++)
{
if(map_msg.data[i] == 0)
map->cells[i].occ_state = -1;
else if(map_msg.data[i] == 100)
map->cells[i].occ_state = +1;
else
map->cells[i].occ_state = 0;
}
return map;
} 填充完地图，开始均匀采样：
#if NEW_UNIFORM_SAMPLING
// Index of free space
free_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));
#endiffree_space_indices是一个线性容器，利用resize操作把容器的长度删为0。然后遍历每个状态为-1的像素点，如果有则往容器的末尾添加由std:make_pair所构建的包含i与j位置坐标的结构体。继续往下看，开始创建粒子滤波器：
// Create the particle filter
pf_ = pf_alloc(min_particles_, max_particles_,
alpha_slow_, alpha_fast_,
(pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
(void *)map_);
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;
// Initialize the filter
updatePoseFromServer();
pf_vector_t pf_init_pose_mean = pf_vector_zero();
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
pf_init_ = false;来看到pf_alloc函数：
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
int i, j;
pf_t *pf;
pf_sample_set_t *set;
pf_sample_t *sample;
srand48(time(NULL));
pf = calloc(1, sizeof(pf_t));
pf->random_pose_fn = random_pose_fn;
pf->random_pose_data = random_pose_data;
pf->min_samples = min_samples;
pf->max_samples = max_samples;
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
pf->current_set = 0;
for (j = 0; j < 2; j++)
{
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc(3 * max_samples);
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}
pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;
//set converged to 0
pf_init_converged(pf);
return pf;
}输入的参数有min_samples,max_samples,alpha_slow,slpha_fast,random_pose_fn,random_pose_data,一开始先创造了一个pf来存关于粒子的参数，然后给每个粒子分配初始权重和方向。并从yaml中装载滤波器的均值和方差。然后初始化了里程计模型和雷达模型。
// Odometry
delete odom_;
odom_ = new AMCLOdom();
ROS_ASSERT(odom_);
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
// Laser
delete laser_;
laser_ = new AMCLLaser(max_beams_, map_);
ROS_ASSERT(laser_);首先看AMCLOdom类的构造函数
回到构造函数，这三行代码是用来装载参数服务器的
dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~"));
dynamic_reconfigure::Server::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
最后创建了一个定时器来定时运行AmclNode::checkLaserReceived这个函数，使用了boost::bind
check_laser_timer_ = nh_.createTimer(laser_check_interval_,
boost::bind(&AmclNode::checkLaserReceived, this, _1));main函数到此结束，开始分析回调函数。
首先是最主要的回调函数laserReceived
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,this, _1));AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id);
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;
if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
geometry_msgs::PoseStamped ident;
ident.header.frame_id = laser_scan_frame_id;
ident.header.stamp = ros::Time();
tf2::toMsg(tf2::Transform::getIdentity(), ident.pose);
geometry_msgs::PoseStamped laser_pose;
try
{
this->tf_->transform(ident, laser_pose, base_frame_id_);
}
catch(tf2::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan_frame_id.c_str(),
base_frame_id_.c_str());
return;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.pose.position.x;
laser_pose_v.v[1] = laser_pose.pose.position.y;
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan_frame_id] = laser_index;
} else {
laser_index = frame_to_laser_[laser_scan_frame_id];
}
pf_vector_t pose;
if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_))
{
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
return;
}
pf_vector_t delta = pf_vector_zero();
if(pf_init_)
{
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
bool update = fabs(delta.v[0]) > d_thresh_ ||
fabs(delta.v[1]) > d_thresh_ ||
fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
}
bool force_publication = false;
if(!pf_init_)
{
pf_odom_pose_ = pose;
pf_init_ = true;
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
else if(pf_init_ && lasers_update_[laser_index])
{
AMCLOdomData odata;
odata.pose = pose;
odata.delta = delta;
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
}函数的一开始就将雷达坐标系进行赋值，如果没有map的信息也不执行此程序。
std::map< std::string, int > frame_to_laser_;
if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end())在map中查找key=laser_scan_frame_id的数据，如果查不到就往下执行。
amcl构造函数结束。