slam-gmapping 源码阅读

gmapping 输入输出

SlamGMapping类

构造函数

• SlamGMapping() 
  默认构造函数完成map_to_odom 的tf初始,激光话题订阅,变换线程,随机种子,以及init
• SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh)
• SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):

---

初始化函数 init()

1.生成GridSlamProcesssor对象以及tf广播对象
2.设置Gmapping参数

---

启动函数 startLiveSlam()

• 发布/map

• laserCallback

• publishLoop

  发布tf变换 map_to_odom

• mapCallback

  提供服务,获取实时地图?

---

---

laserCallback

• laserCallback 激光数据处理 
   

• initMapper 如果是第一个激光数据，初始化map 
  一些重要参数的初始化，将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.调用采样一次函数，设置随机种子

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

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

• updateMap(*scan); 
  如果没有得到地图或者当前时间达到了更新时间 更新地图 updateMap

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

• getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t) 
  t时刻激光中心位置在里程计坐标下的位姿,返回至gmap_pose

  参考文献 
  gmapping 分析

---