Gmapping 源码阅读(1) ————源码入口

一、整体框架结构介绍

gmapping源码文件有两个分别为slam_gmapping 和 openslam_gmapping两部分。

• slam_gmapping程序包的主要作用是为激光建图的应用层实现激光点云和里程计数据的读取、粒子滤波以及地图的更新。实现话题的订阅和发布。
• openslam_gmapping程序包负责算法具体实现，包括栅格地图的构建、粒子滤波、扫描匹配等方法，为应用程序提供接口。

二、Main函数

main.cpp: main函数为程序的入口，完成了Node的初始化，SlamGMapping类的调用，进入startLiveSlam()成员函数。

int main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping"); //节点初始化
  
  //调用SlamGMapping类
  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();

  return(0);
}

在main函数中我们可以看到，程序首先初始化一个SlamGMapping类对象，并且利用这个对象运行类成员函数startLiveSlam()。因此我们看一下SlamGMapping这个类。

三、SlamGMapping构造函数

SlamGMapping中共定义了三个构造函数，分别为SlamGMapping(); SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh); SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);main函数中使用了无参数的构造函数

SlamGMapping::SlamGMapping():
  map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
  laser_count_(0), private_nh_("~"), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
  seed_ = time(NULL);
  init();
}

该构造函数分别初始化了map_to_odom,laser_count_,private_nh_,scan_filter_sub_,transform_thread_,send_三个变量，并且运行init()函数。

tf::Transform map_to_odom_; 地图到里程计的坐标变换

int laser_count_; 激光雷达数量

ros::NodeHandle private_nh_; ros句柄

message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;

boost::thread* transform_thread_; 线程

unsigned long int seed_;

四、init()函数

init()函数主要就是初始化参数以及获取launch文件传递进来的一些函数。

void SlamGMapping::init()
{
  // log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);

  // The library is pretty chatty
  //gsp_ = new GMapping::GridSlamProcessor(std::cerr);
  gsp_ = new GMapping::GridSlamProcessor();//构造GridSlamProcessor类 利用粒子滤波
  ROS_ASSERT(gsp_);

  tfB_ = new tf::TransformBroadcaster();//创建TransformBroadcaster
  ROS_ASSERT(tfB_);

  gsp_laser_ = NULL;
  gsp_odom_ = NULL;

  got_first_scan_ = false;
  got_map_ = false;
  
  // Parameters used by our GMapping wrapper
  if(!private_nh_.getParam("throttle_scans", throttle_scans_))
    throttle_scans_ = 1;
  if(!private_nh_.getParam("base_frame", base_frame_))
    base_frame_ = "base_link";
  if(!private_nh_.getParam("map_frame", map_frame_))
    map_frame_ = "map";
  if(!private_nh_.getParam("odom_frame", odom_frame_))
    odom_frame_ = "odom";

  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);

  double tmp;
  if(!private_nh_.getParam("map_update_interval", tmp))
    tmp = 5.0;
  map_update_interval_.fromSec(tmp);
  
  // Parameters used by GMapping itself
  // Parameters used by GMapping itself #GMapping 初始化所用的各种参数
  maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
  if(!private_nh_.getParam("minimumScore", minimum_score_))
    minimum_score_ = 0;
  if(!private_nh_.getParam("sigma", sigma_))
    sigma_ = 0.05;
  if(!private_nh_.getParam("kernelSize", kernelSize_))
    kernelSize_ = 1;
  if(!private_nh_.getParam("lstep", lstep_))
    lstep_ = 0.05;
  if(!private_nh_.getParam("astep", astep_))
    astep_ = 0.05;
  if(!private_nh_.getParam("iterations", iterations_))
    iterations_ = 5;
  if(!private_nh_.getParam("lsigma", lsigma_))
    lsigma_ = 0.075;
  if(!private_nh_.getParam("ogain", ogain_))
    ogain_ = 3.0;
  if(!private_nh_.getParam("lskip", lskip_))
    lskip_ = 0;
  if(!private_nh_.getParam("srr", srr_))
    srr_ = 0.1;
  if(!private_nh_.getParam("srt", srt_))
    srt_ = 0.2;
  if(!private_nh_.getParam("str", str_))
    str_ = 0.1;
  if(!private_nh_.getParam("stt", stt_))
    stt_ = 0.2;
  if(!private_nh_.getParam("linearUpdate", linearUpdate_))
    linearUpdate_ = 1.0;
  if(!private_nh_.getParam("angularUpdate", angularUpdate_))
    angularUpdate_ = 0.5;
  if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
    temporalUpdate_ = -1.0;
  if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
    resampleThreshold_ = 0.5;
  if(!private_nh_.getParam("particles", particles_))
    particles_ = 30;
  if(!private_nh_.getParam("xmin", xmin_))
    xmin_ = -100.0;
  if(!private_nh_.getParam("ymin", ymin_))
    ymin_ = -100.0;
  if(!private_nh_.getParam("xmax", xmax_))
    xmax_ = 100.0;
  if(!private_nh_.getParam("ymax", ymax_))
    ymax_ = 100.0;
  if(!private_nh_.getParam("delta", delta_))
    delta_ = 0.05;
  if(!private_nh_.getParam("occ_thresh", occ_thresh_))
    occ_thresh_ = 0.25;
  if(!private_nh_.getParam("llsamplerange", llsamplerange_))
    llsamplerange_ = 0.01;
  if(!private_nh_.getParam("llsamplestep", llsamplestep_))
    llsamplestep_ = 0.01;
  if(!private_nh_.getParam("lasamplerange", lasamplerange_))
    lasamplerange_ = 0.005;
  if(!private_nh_.getParam("lasamplestep", lasamplestep_))
    lasamplestep_ = 0.005;
    
  if(!private_nh_.getParam("tf_delay", tf_delay_))
    tf_delay_ = transform_publish_period_;

}

五、 startLiveSlam()函数

void SlamGMapping::startLiveSlam()
{
  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);//发布entropy话题
  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);//发布map话题
  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);//发布map_metadata话题
  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);//mapCallback()
  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);//订阅scan话题
  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);//激光雷达数据和odom帧数据的变换??
  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));//开启线程laserCallback()

  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));//开启线程publishLoop()
}

该函数从SlamGMapping类中启动了三个类函数分别为: mapCallback(),laserCallback(),publishLoop()。

启动发布了三个话题分别为: /map,/map_metadata,/entropy

启动了一个服务:/dynamic_map

实例化了两个对象:scan_filter_sub_,scan_filter_

publishLoop()单独开启一个线程定时发送tf

该函数订阅了激光扫描、地图等话题，获取到的激光数据后首先通过消息过滤器，实现激光扫描数据的订阅以及tf、frame变换，之后激光数据消息处理回调函数。

后面根据它们使用的位置判断它们的作用先看这几句:

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));

该部分首先订阅scan的话题，然后使用消息过滤器tf::MessageFilter同步激光雷达与里程计的消息。

对于tf::MessageFilter我们简单的说明一下:

消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。 举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。

最后调用laserCallback()函数处理获取并处理激光雷达数据。

下一章，我们将进入laserCallback这个回调函数，并且逐步进入Gmapping的核心代码，了解该算法的运行机制。