AMCL源码阅读（ros2版本）

阅读amcl_ros2版本源码

源码git地址：https://github.com/ros2/navigation.git

本人注释的git地址：https://github.com/adayimaxiga/cartographer_migrate_to_ros2/tree/master/amcl

这个amcl执行过程与ros1版本没啥区别

首先阅读amcl_node.cpp

主函数调用这一句

amcl_node_ptr.reset(new AmclNode(node, use_map_topic));       //这里构造了AMCL类，调用了AMCL构造函数，所有程序入口
           

这个文件结构比较清晰，主函数中调用AmclNode构造函数，这个函数主要干了这么几件事：读取参数，初始化位置，发布/订阅话题（发布话题：amcl_pose，particlecloud）（订阅话题：initialpose，map）（服务：global_localization，request_nomotion_update，set_map）调用关系大概是这样的：

最关键的一个函数是激光雷达话题的回调函数，所有算法在这里运行。来看laserReceived函数都在干啥：

1. 首先获取了laser相对于baselink的tf
2. 然后获取了baselink相对于odom的tf，这样就知道laser在世界坐标系下的位置可以继续进行匹配了。
3. 接下来分为两种情况，pf是否第一次运行，如果是第一次就将里程计值赋给pf_odom_pose_。如果是正常迭代过程，则计算里程计变化值，并判断里程计变化是不是达到了我们要进行更新的阈值。
4. 里程计运动达到更新阈值即调用UpdateAction()函数，打开该函数，这里可以看出来是根据设置的运动预测模型进行采样，这就是粒子滤波的预测模型了。
5. 然后回到laserReceived函数中，读取了激光数据后，将最大范围与最小范围传给amcl_laser。然后由于amcl算法并未对最小距离做处理，因此如果小于range_min就赋值为range_max做与max同样处理。之后处理好的数据调用UpdateSensor，将激光数据上传，我们打开这个函数
6. UpdateSensor对于激光数据处理，根据模型不同调用了pf_update_sensor，一般情况下，激光观测模型为LikelihoodFieldModel这个经典模型。这里根据似然域来做更新。
7. 看一下LikelihoodFieldModel这个函数，这里我理解的是由于里程计运动大于一定值才进行预测及更新，这里会取与里程计时间相近的一次激光，模型里计算流程大概是：对于每一个粒子，首先计算激光坐标系，然后滤去大于max_range的激光点（这里由于上一步的处理，连min_range一起滤掉了），滤去not a number,计算每一束激光落点栅格，计算落点栅格与地图上障碍点最近曼哈顿距离，根据激光测距的高斯模型概率，每个粒子权重为每一束激光单独扫描的乘积，再计算总权重。这里是激光观测模型的计算流程。
8. 回到laserReceived函数中，这里完成更新部分之后进行重采样，重采样函数为pf_update_resample()函数。重采样的方法比较经典，根据w值增加了一部分随机生成的粒子，当drand48<w_diff，随机均匀在地图范围内产生一个粒子。这里是调用uniformPoseGenerator产生的。而drand48>w_diff时，将PDF积分得到CDF，然后drand48来对应离散CDF中采样，这样drand48落在哪个点上就把哪个粒子复制一份，将所有粒子权重置为1，完成重采样。然后对每个粒子权重进行归一化，重置fast和slow，重新计算统计数据（均值，方差）
9. 回到laserReceived函数中，最后一步是发布计算好的点云数据。