amcl的总体业务逻辑
在浅析amcl_demo的时候，我们已经了解到amcl在这个demo中扮演了机器人定位器的角色。 amcl_demo通过包turtlebot_navigation的amcl.launch.xml文件，定义了运行amcl的配置参数， 并通过类似下述的代码运行amcl节点。
<!-- 定义了各种输入参数arg，省略之 -->
<node pkg="amcl" type="amcl" name="amcl">
<!-- 定义了各种运行参数param，省略之 -->
</node>
本文是amcl源码解析部分的开篇，从总体上分析其源码结构和业务逻辑。
$ git clone https://github.com/ros-planning/navigation
$ cd navigation/amcl
$ tree .
.
├── CMakeLists.txt
├── include
│ └── amcl
│ ├── map
│ │ └── map.h
│ ├── pf
│ │ ├── eig3.h
│ │ ├── pf.h
│ │ ├── pf_kdtree.h
│ │ ├── pf_pdf.h
│ │ └── pf_vector.h
│ └── sensors
│ ├── amcl_laser.h
│ ├── amcl_odom.h
│ └── amcl_sensor.h
├── package.xml
└── src
├── amcl
│ ├── map
│ │ ├── map.c
│ │ ├── map_cspace.cpp
│ │ ├── map_draw.c
│ │ ├── map_range.c
│ │ └── map_store.c
│ ├── pf
│ │ ├── eig3.c
│ │ ├── pf.c
│ │ ├── pf_draw.c
│ │ ├── pf_kdtree.c
│ │ ├── pf_pdf.c
│ │ └── pf_vector.c
│ └── sensors
│ ├── amcl_laser.cpp
│ ├── amcl_odom.cpp
│ └── amcl_sensor.cpp
└── amcl_node.cpp
1. 源码结构
如右侧代码所示，amcl是navigation项目中的一个包，我们通过从github上将之拉下来，查看其目录结构。这里省略了与总体业务无关的文件和目录。
在include和src两个子目录中，分别有三个子目录map、pf和sensors，它们分别对应着amcl的三个模块，用于描述地图和传感器信息，提供粒子滤波器。后面我们将分别用三篇文章来介绍它们。
从CMakeLists.txt中我们可以看到，amcl将map、pf、sensors三个目录下的源文件分别编译成为三个动态链接库。
add_library(amcl_pf src/amcl/pf/pf.c ##省略其他文件##)
add_library(amcl_map src/amcl/map/map.c ##省略其他文件##)
add_library(amcl_sensors src/amcl/sensors/amcl_sensor.cpp
##省略其他文件##)
target_link_libraries(amcl_sensors amcl_map amcl_pf)
此外，还通过宏add_executable指定了amcl节点的编译指令，使用add_dependencies指出编译该节点所需的依赖关系。 并且使用target_link_libraries说明编译amcl所需的链接库。
add_executable(amcl src/amcl_node.cpp)
add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS})
target_link_libraries(amcl amcl_sensors amcl_map amcl_pf
${Boost_LIBRARIES}
${catkin_LIBRARIES})
源文件amcl_node.cpp则完成了总体的业务逻辑。在该文件中定义了整个节点运行的入口main函数。在main函数中，首先完成ROS系统的初始化，并定义一个节点句柄nh。
int main(int argc, char** argv) {
ros::init(argc, argv, "amcl");
ros::NodeHandle nh;
接着，根据Linux系统的系统调用signal指定信号SIGINT的处理方式。在很多资料中，都将signal称为是软中断信号，或者简称信号机制。 它是一种类似于处理器中断的异步事件机制。可以用于进程之间的异步通信，内核也会因为一些系统事件给特定的进程发送信号。
进程接收到信号之后，可以选择忽略它，也可以指定一个信号响应函数来完成用户期望的功能。更多的情况是，操作系统为信号定义了一些默认的操作。 这里通过系统调用signal为SIGINT信号注册了一个通过函数sigintHandler来完成的用户行为。这个SIGINT信号就是按下键盘<Ctrl+c>后产生的信号，通常用于终结一个进程。 响应函数sigintHandler如右侧代码所示，只是保存当前位姿到服务器上，然后关闭ROS系统退出。
|
|
在上面的代码片段的第8行中，通过全局的boost智能指针构建了一个新的AmclNode对象。智能指针的声明如上面右边代码的第5行所示。boost是一个C++标准库的扩展库，提供了大量的工具， 其中shared_ptr就是一个智能指针，它对C++原生的指针类型进行了封装，可以智能的释放不被需要的内存。在C++11的标准中，智能指针就作为标准库来实现了。
其实智能指针的概念，我们并不陌生，在GMapping中就实现了一种称为autoptr的智能指针。 其思想与boost的设计是一致的，都是使用一个计数器来记录对象的引用数量，当计数为0时释放对象的内存。
然后，amcl根据输入参数来决定运行方式。从代码中，我们可以看到amcl支持数据回放的，运行时只需要通过参数"--run-from-bag"指定bag文件即可。 这里的两个分支语句都将导致amcl进入业务循环中，只是第9行使用的是ROS的输入，而第11行则是回放数据。
if (argc == 1)
ros::spin(); // run using ROS input
else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
amcl_node_ptr->runFromBag(argv[2]);
当因为一些事件导致ros::spin()和runFromBag()两种业务循环退出时，需要释放指针，并退出程序。
// Without this, our boost locks are not shut down nicely
amcl_node_ptr.reset();
// To quote Morgan, Hooray!
return(0);
}
2. 业务对象AmclNode的构造
从上面的main函数分析，我们可以看到amcl的所有业务逻辑都是由AmclNode类完成的。下面的代码片段是其构造函数以及部分成员变量的初始化，相关变量的类型和意义如其后的注释所示。
AmclNode::AmclNode() :
sent_first_transform_(false), // bool, 是否发送过坐标变换
latest_tf_valid_(false), // bool, 坐标变换是否可用
map_(NULL), // map_t*, 地图对象指针
pf_(NULL), // pf_t*, 粒子滤波器对象指针
resample_count_(0), // int, 重采样计数
odom_(NULL), // AMCLOdom*, 里程计对象
laser_(NULL), // AMCLLaser*, 激光雷达对象
private_nh_("~"), // ros::NodeHandle, 局部ROS句柄
initial_pose_hyp_(NULL), // amcl_hyp_t*, 初始位姿假设
first_map_received_(false), // bool, 是否接收到地图
first_reconfigure_call_(true) // 初次调用参数重配置 {
在构造函数的初始，先对信号量configuration_mutex_加锁，防止与动态参数配置冲突。同时通过private_nh_局部ROS句柄获取amcl的运行参数，它们的数据类型和意义， 我们已经在amcl_demo之机器人定位器——amcl一文中详细列举了。
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
private_nh_.param("use_map_topic", use_map_topic_, false);
// 其余参数，略。
updatePoseFromServer();
在众多的从参数服务器上获取运行参数的语句之后，调用了一个成员函数updatePoseFromServer，该函数就是用来从参数服务器上获取机器人的初始位姿和位姿误差的协方差矩阵的。
接着构建tf2的相关对象，其中tfb_是一个坐标变换的广播器，tfl_则是一个坐标变换的监听器，tf_则是一个坐标变换的历史缓存。 tf2是ROS中标准的坐标变换实现，早期的ROS系统中使用的是tf库，目前官方在极力推广tf2，tf将不再维护。
|
|
然后发布amcl_pose和particlecloud两个主题，分别用于输出机器人位姿估计和粒子集合。
pose_pub_ = nh_.advertise("amcl_pose", 2, true);
particlecloud_pub_ = nh_.advertise("particlecloud", 2, true);
接着注册三个服务，其中global_localization用于获取机器人的全局定位，request_nomotion_update则用于手动的触发粒子更新并发布新的位姿估计，set_map用于设定机器人位姿和地图信息。
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);
然后构建激光传感器的消息过滤器对象和tf2的过滤器，并注册回调函数laserReceived。这里的message_filter为ROS系统提供了一些通用的消息过滤方法， 它对接收到的消息进行缓存，只有当满足过滤条件后才输出，在需要消息同步的时候应用比较多。这里主要是同时监听激光扫描消息和里程计坐标变换，同步两者的输出。 这一套路在GMapping中也有用到。
laser_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_);
laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived, this, _1));
订阅用于初始化机器人位姿估计的主题initialpose:
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
根据运行参数use_map_topic_获取地图，或者订阅地图主题，或者通过requestMap请求"static_map"服务。
if(use_map_topic_)
map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
else
requestMap();
下面这几行代码用于实现amcl的动态参数配置，与总体业务逻辑无关。
dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
定义一个计时器用于每隔15s检查一次激光雷达的接收数据，如果期间没有收到新的数据给出告警。
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);
}
3. 地图与粒子滤波器的构建
主题"map"的回调函数mapReceived，以及请求"static_map"服务的requestMap在接收到地图数据之后，都会调用函数handleMapMessage进一步完成AmclNode的初始化工作。 下面是该函数代码片段，它以地图数据消息的引用为输入参数构建了地图对象，里程计对象和雷达传感器对象，并完成了滤波器的初始化工作。 在函数的一开始对配置信号量加锁，并输出一些关于地图尺寸和分辨率的日志。
void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) {
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
// 省略一些日志输出
在释放了与地图相关的内存和对象之后，通过函数convertMap将地图消息转换成amcl中的地图数据结构。
freeMapDependentMemory();
lasers_.clear();
lasers_update_.clear();
frame_to_laser_.clear();
map_ = convertMap(msg);
接着构建粒子滤波器对象，并完成初始化。
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;
然后，构建里程计对象并配置里程计模型。
odom_ = new AMCLOdom();
odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
构建雷达传感器对象，并根据运行参数laser_model_type_构建不同模型的雷达。
laser_ = new AMCLLaser(max_beams_, map_);
if(laser_model_type_ == LASER_MODEL_BEAM)
laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, 0.0);
else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_);
else
laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_);
最后考虑到，在接收到地图数据之前，有可能已经接收到了机器人的初始位姿，这里调用函数applyInitialPose处理这一情况。
applyInitialPose();
}
4. AmclNode的业务逻辑
Amcl的业务逻辑总体就是在一个四五百行的巨大函数laserReceived中实现的，它就是刚刚我们提到的消息过滤器laser_scan_filter_的回调函数。 由于这个函数实在太长，这里我们将删除一些与主题业务不太相关的语句。
如下面的代码片段所示，该函数以雷达扫描数据的指针为输入参数。在函数的一开始，从扫描数据中获取雷达消息ID。同时用变量last_laser_received_ts_记录下当前的系统时间， 它用于判定是否长时间未接收到雷达数据。此外如果没有地图对象，将直接退出。
void 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;
如下面右边的代码片段所示，AmclNode借助一些vector和map的容器，来支持多传感器。它使用lasers_记录下当前构建的雷达对象，lasers_update_标记雷达的更新状态， 并通过一个string到int的map建立其雷达坐标ID到雷达对象在lasers_中的对应关系。
通过在frame_to_laser_中查找雷达的坐标ID，如果之前没有收到该雷达的消息，将新建一个对象保存在lasers_中，并相应的在lasers_update_中添加对应更新状态， 建立映射关系。否则，我们就直接通过frame_to_laser_获取雷达对象的索引。
|
|
在讨论完雷达坐标系之后，AmclNode通过成员函数getOdomPose获取接收到雷达数据时刻的里程计位姿。如果无法获取则报错退出。
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;
}
如果里程计的数据显示机器人已经发生了明显的位移或者旋转，就标记所有的雷达更新标记为true。
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]);
if (fabs(delta.v[0]) > d_thresh_ || fabs(delta.v[1]) > d_thresh_ || fabs(delta.v[2]) > a_thresh_ || m_force_update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
m_force_update=false;
}
根据刚刚更新标识，我们调用里程计对象的UpdateAction接口完成运动更新。(按:pf_init_虽然字面意思上是判定粒子滤波器是否得到了初始化，实际上源码中只是完成了一些状态变量的初始赋值而已， 这里略去。)
if(pf_init_ && lasers_update_[laser_index]) {
AMCLOdomData odata;
odata.pose = pose;
odata.delta = delta;
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
}
接下来，我们根据激光的扫描数据更新滤波器。首先构建AMCLLaserData对象，并指定传感器对象和量程，然后将接收到的传感器数据拷贝到ldata对象中， 最后通过激光传感器对象的UpdateSensor接口完成粒子滤波器的测量更新。
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// 省略雷达数据的拷贝
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
然后清除lasers_update_的标记，并更新滤波器的里程计位姿。根据重采样计数器和设定的阈值，触发对滤波器的重采样。
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
if(!(++resample_count_ % resample_interval_)) {
pf_update_resample(pf_);
resampled = true;
}
至此，amcl的主业务逻辑就基本结束了。剩下的内容就是：获取当前的粒子集合，从中提炼出机器人的位姿估计，并将这些消息通过各个主题发布出去。
pf_sample_set_t* set = pf_->sets + pf_->current_set;
// 省略构建和发布消息的语句
}
5. 完
本文中，我们分析了amcl包的源码结构。并详细解析了AmclNode的初始化过程和主体的业务逻辑。完成主体业务的laserReceived函数写的太冗长了，我们删减了很多语句， 其最核心的就是分别调用里程计对象完成运动更新，调用传感器对象完成测量更新，并在必要时刻进行重采样。