ros slam_gmapping 算法梳理

gampping 建图：

slam_gmapping包也是依赖开源openslam_gmapping库。
源代码网址：
slam_gmapping:https://github.com/ros-perception/slam_gmapping
openslam_gmapping:https://github.com/ros-perception/openslam_gmapping
gmapping 是一种基于粒子滤波的算法，它依赖于里程计数据。

缺点：因为严重依赖里程计数据，因此无法适应不平坦的区域（无人机场景），在大场景中，粒子数较多，特别消耗硬件资源。
因此，适用场景为室内小场景，平坦地面的环境。
源码结构组织：
gmapping 底层计算调用的是openslam-gmapping 里的算法

1、SlamGMapping类
三个构造函数：
(1)、SlamGMapping():
默认构造函数完成map_to_odom 的tf初始,激光话题订阅,变换线程,随机种子,以及初始化。
(2)、SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):

(3)、SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
2、初始化函数init():
(1)、生成GridSlamProcesssor对象以及tf广播对象
(2)、设置Gmapping参数
3、启动函数 startLiveSlam()，开启建图
(1)、发布三个话题：
/entropy:机器人姿态
/map:建图数据 用网格数描述，其值为0~255
/map_metadata:地图描述信息
(2)、发布一个服务：
/dynamic_map:获取地图数据，
(3)、订阅
LaserScan
tf转换后的数据
(4)、开启一个线程
不断广播map_to_odom
4、getOdomPose函数
获取机器人的初始位置（一般仅执行一次）
5、LaserCallback

6、initMapper函数 的传入参数为scan
一旦第一次获取到scan数据，就会调用initMapper函数，将slam里的参数传递到 openslam 里 ，设定坐标系，坐标原点，以及采样函数随机种子的初始化等等，里面还调用了 GridSlamProcessor::init 函数,这个初始化了粒子数，子地图大小。

(1).利用监听tf_ 获取激光相对于基座的位姿laser_pose
(2).创建一个激光上方1m的点up(base_frame下 （0，0，lase_pose.z+1））,然后转化为相对于激光的坐标
(3).如果up的z轴坐标和1有相差，则说明激光未安装水平。则初始化失败
(4).获取激光中心位置 ，并将原始激光数据赋值给laser_angles (类型为vector);
(5).初始化激光传感器模型对象RangeSensor ，里程计模型对象OdometrySensor
(6).利用getOdomPose 设置初始激光位姿，设置失败则设为zero(0,0,0)
(7).设置处理器的匹配参数，更新距离、更新频率、采样范围、采样步骤等参数
(8).调用采样一次函数，设置随机种子

7、 addScan函数
addScan(scan, odom_pose)
处理当前的激光数据和里程计位姿。addScan这个函数要转到pf的核心代码了 ，将调用processScan（reading）。processScan核心函数

(1).如果角度增量为负,反转数据顺序.将scan距离数据放置到double数组中,若出现不合理值,则均置为最大值
(2).生成RangeReading 对象
(3).调用激光数据处理对象即processScan

8、updateMap

(1).生成ScanMatcher对象,matcher初始化包括 ,激光参数,范围参数,产生地图true
(2).取最优粒子，根据权重和weightSum 判断（最大）.计算平均值(熵),若大于0,发布
(3).若地图初始为(0,0,0)
(4).得到机器人最优的携带地图路径,重新计算栅格单元的概率.
(5).地图尺寸,可能需要膨胀
(6).确定地图的未知区域,自由区域,障碍.即占据概率大于阈值,为障碍设置为100.概率小于0,为自由区设置为-1.未知为0
(7).发布map话题