gmapping 分析

这一篇先讲我对gmapping源码的理解(难免有错，欢迎指正，相互学习)。

参考论文:
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
参考博客：
http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理讲解
http://blog.csdn.net/u010545732/article/details/17462941 pf代码实现
http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析
其他参考 ：
http://ishare.iask.sina.com.cn/f/24615049.html

从ros官网上下载 slam_gmapping 包以及在openslam （ http://openslam.org/ ）上下载openslam_gmapping包。
为了方便的阅读源码，这里强力推荐一款源码阅读软件 understand （聪明的你一定找的到资源），可以方便实现各种跳转与生成图、表、树，流程等。
废话不多说了，开始看源码，对于我这种c++都没有过关的菜鸟，看着大几千行的c++的代码，简直是身体和精神上的蹂躏。
先说说 slam_gmapping 包与openslam_gmapping包
进入slam_gmapping 的main.cpp文件的关系，slam_gmapping 是openslam_gampping在ros下的二次封装，你可以直接用这个包，而真正的核心代码实现都在openslam_gampping里面。

进入代码
先用understand 看看代码的调用关系。 （调用太复杂了，图太大，我就截取了一小部分）

转到

gn.startLiveSlam();
{
  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));

  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

也没写啥，主要就是一些消息的回调以及发布一些服务，重点在

void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
   /*每隔throttle_scans_ （默认值 1）帧数据计算一次，限流作用*/
  if ((laser_count_ % throttle_scans_) != 0)  
    return;

  static ros::Time last_map_update(0,0);

  /* We can't initialize the mapper until we've got the first scan */
  if(!got_first_scan_)
  {
 /*一些重要参数的初始化，将slam里的参数传递到 openslam 里 ，设定坐标系，坐标原点，以及采样函数随机种子的初始化，等等
 里面还调用了 GridSlamProcessor::init ,这个初始化了粒子数，子地图大小 */
    if(!initMapper(*scan))
      return;
    got_first_scan_ = true;
  }

  GMapping::OrientedPoint odom_pose;
/**
pay attention： addScan这个函数*要转到pf的核心代码了 ，将调用processScan
*/
  if(addScan(*scan, odom_pose))   
  {
    ROS_DEBUG("scan processed");

    GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
    ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose