终于来到我们的源码部分了，话不多说，直接开始。

1、初始化函数init（）

这部分就是给建图设置必要的参数

void MySlamGMapping::init()
{
    gsp_ = new GMapping::GridSlamProcessor();   //GMapping::GridSlamProcessor* gsp_     开辟一个动态内存，用于存储gmapping的数据（栅格数据)          
    tfB_ = new tf::TransformBroadcaster();      //发布tf变换  tf::TransformBroadcaster* tfB_

    got_first_scan_ = false;        //用作判断，是否接收到第一个扫描点
    got_map_ = false;               //用作判断，是否接收到地图信息

    if(!private_nh_.getParam("map_frame", map_frame_))  //获取参数，map_frame_是地图坐标系的名字，如果没有设置，默认为map
        map_frame_ = "map";
    if(!private_nh_.getParam("odom_frame", odom_frame_))  //获取参数，odom_frame_是odom坐标系的名字，如果没有设置，默认为odom
        odom_frame_ = "odom";
    if(!private_nh_.getParam("scan_topic", scan_topic_))  //获取参数，scan_topic_是扫描点的topic名字，如果没有设置，默认为scan
        scan_topic_ = "scan";

    private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);//获取参数，transform_publish_period_是发布tf变换的周期，默认为0.05


    double tmp;
    if(!private_nh_.getParam("map_update_interval", tmp))  //获取参数，map_update_interval_是地图更新的周期， 如果没有设置
        tmp = 5.0;                          //那么就设置为5.0
    map_update_interval_.fromSec(tmp);  //将tmp转换为ros::Duration类型，并赋值给map_update_interval_  ros::Duration是一个类，用于记录时间的类型，

    maxUrange_ = 0.0;  maxRange_ = 0.0; 
    if(!private_nh_.getParam("minimumScore", minimum_score_))  //获取参数，minimum_score_是最小的分数，如果没有设置，默认为0
        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_) ) //scan-match中的optimization距离优化步长
        lstep_ = 0.05;
    if(!private_nh_.getParam("astep", astep_))   //scan-match中的optimization角度优化步长
        astep_ = 0.05;
    if(!private_nh_.getParam("iterations", iterations_))  //scan-match中的optimization迭代次数
        iterations_ = 5;
    if(!private_nh_.getParam("lsigma", lsigma_))       //scan-match的似然计算的标准偏差
        lsigma_ = 0.075;
    if(!private_nh_.getParam("ogain", ogain_))     //评估可能性时使用的增益，用于平滑重采样效果
        ogain_ = 3.0;
    if(!private_nh_.getParam("lskip", lskip_))    //对于一帧激光雷达数据来说 只取每第(n+1)个激光束  这个是相对于scan-match来说的
        lskip_ = 0;
    if(!private_nh_.getParam("srr", srr_))       //机器人的运动模型的相关的噪声参数，inear noise component (x and y)
        srr_ = 0.1;
    if(!private_nh_.getParam("srt", srt_))        //机器人的运动模型的相关的噪声参数，angular noise component (theta)
        srt_ = 0.2;
    if(!private_nh_.getParam("str", str_))       //机器人的运动模型的相关的噪声参数，linear -> angular noise component
        str_ = 0.1;
    if(!private_nh_.getParam("stt", stt_))      //机器人的运动模型的相关的噪声参数，angular -> linear noise component
        stt_ = 0.2;


    if(!private_nh_.getParam("linearUpdate", linearUpdate_))  //只有当机器人至少移动了linearUpdate米，机器人才会处理新的测量数据，进行scan-match
        linearUpdate_ = 1.0;
    if(!private_nh_.getParam("angularUpdate", angularUpdate_))  //只有当机器人至少旋转了angularUpdate弧度，机器人才会处理新的测量数据，进行scan-match
        angularUpdate_ = 0.5;
    if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))  //如果距离上次scan-match时间大于temporalUpdate（秒），则进行scan-match，负值则不更新
        temporalUpdate_ = -1.0;

        //注意上面三个条件，只要一个符合就会进行scan-match

    if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))  //粒子重新采样的阈值。更高意味着更频繁的重采样
        resampleThreshold_ = 0.5;
    if(!private_nh_.getParam("particles", particles_))            //粒子滤波的粒子个数
        particles_ = 30;
    if(!private_nh_.getParam("xmin", xmin_))                    //初始地图x方向最小尺寸
        xmin_ = -100.0;
    if(!private_nh_.getParam("ymin", ymin_))                     //初始地图y方向最小尺寸
        ymin_ = -100.0;
    if(!private_nh_.getParam("xmax", xmax_))                     //初始地图x方向最大尺寸
        xmax_ = 100.0;
    if(!private_nh_.getParam("ymax", ymax_))                     //初始地图y方向最大尺寸
        ymax_ = 100.0;
    if(!private_nh_.getParam("delta", delta_))                   //栅格地图分辨率，每个栅格为边长0.05m的正方形 
        delta_ = 0.05;
    if(!private_nh_.getParam("occ_thresh", occ_thresh_))          //栅格地图的占用阈值，超过则认为栅格被占用 
        occ_thresh_ = 0.25;

    if(!private_nh_.getParam("tf_delay", tf_delay_))   //tf延迟时间，单位是秒
        tf_delay_ = transform_publish_period_;       //transform_publish_period_默认是0.05秒
        
    std::cout<<"MySlamGMapping::init finish"<<std::endl;  
}

2、在线建图startLiveSlam ()

具体实现：
1、创建两个发布者，发布地图信息：map是一个地图，map_metadata是地图的元数据 元数据是指地图的一些信息，比如地图的尺寸，地图的分辨率等等
2、创建服务端ss_,调用回调函数mapcallback()
3、使用消息过滤器订阅激光数据，然后用该订阅器构建消息过滤器，将消息转换到目标坐标系，调用回调函数lasercallback(),完成地图的更新，最后调用publishloop（）循环发布。
这部分使用了三个回调函数mapcallback()，lasercallback()，publishloop（）后面会依次讲解。

void MySlamGMapping::startLiveSlam()  
{
    //建图所构造的函数  ros::Publisher sst_   ros::Publisher sstm_  ros::ServiceServer ss_   ros::NodeHandle node_
    sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);  //发布者  发布地图信息，  OccupancyGrid是一个消息类型，用于发布地图信息 发布地图信息的频率是1Hz
    sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true); //发布者  发布地图信息，  MapMetaData是一个消息类型，用于发布地图信息  和map的区别是map是一个地图，map_metadata是地图的元数据  元数据是地图的一些信息，比如地图的尺寸，地图的分辨率等等
    ss_ = node_.advertiseService("dynamic_map", &MySlamGMapping::mapCallback, this);  //服务端  dynamic_map是一个服务话题名称  mapcallback是一个回调函数，用于发布地图信息  
    {
        //订阅激光数据，同时和odom_frame之间的转换同步   （激光数据处理）
        //首先用对象scan_filter_sub_订阅了主题"scan"，以监听激光传感器的扫描值。然后用该订阅器构建消息过滤器， 让它同时监听激光扫描消息和里程计坐标变换。
        //最后注册回调函数SlamGMapping::laserCallback，每当传感器的数据可以转换到目标坐标系上时，就调用该函数完成地图的更新。
        //message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_
        //tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
        /*
        1、消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。 
        举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。
             message_filters::Subscriber<std_msgs::UInt32> sub(nh, "my_topic", 1);
             sub.registerCallback(myCallback);
        等同于ros::Subscriber sub = nh.subscribe("my_topic", 1, myCallback);
        
        2、tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）。
        tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，
        调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

        */
                                                                                         //scan_topic_ = "scan"
        scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, scan_topic_, 5);  //订阅激光数据，同时和odom_frame之间的转换同步   （激光数据处理）  参数1：节点对象，参数2：激光数据的主题，参数3：订阅激光数据的频率，参数4：是否阻塞，默认是 true 参数5：回调函数，参数6：回调函数的指针，参数7：回调函数的指针的指针  订阅的话题是scan_topic_,是谁发布的这个话题
        scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);  // 通过消息过滤器构建消息过滤器， 让它同时监听激光扫描消息和里程计坐标变换。  功能是将得到的激光数据转换到里程计坐标系下
        // 通过boost::bind开启两个线程：laserCallback()和 publishLoop()，
        scan_filter_->registerCallback(boost::bind(&MySlamGMapping::laserCallback, this, _1));  //注册回调函数SlamGMapping::laserCallback，每当传感器的数据可以转换到目标坐标系上时，就调用该函数完成地图的更新。  laserCallback()函数订阅激光数据，并将其转换到目标坐标系下，然后调用updateMap()函数更新地图。

        std::cout <<"Start Subscribe LaserScan & odom!!!"<<std::endl;
    }
       //发布转换关系的线程  以transform_publish_period_为周期发布坐标变换
    transform_thread_ = new boost::thread(boost::bind(&MySlamGMapping::publishLoop, this, transform_publish_period_));  //发布转换关系的线程  以transform_publish_period_为周期发布坐标变换  bind

    std::cout <<"Start transform_thread "<<std::endl;
}

3、mapcallback()

功能：获取地图信息，将地图信息赋值给res,res是服务的响应端.

bool MySlamGMapping::mapCallback(nav_msgs::GetMap::Request  &req,
                               nav_msgs::GetMap::Response &res)   //地图回调函数  功能：获取地图信息
{
    boost::mutex::scoped_lock map_lock (map_mutex_);  //加锁
    if(got_map_ && map_.map.info.width && map_.map.info.height)  //如果地图已经构建好了，并且宽度和高度都不为0
    {
        res = map_;  //将地图信息赋值给res  该函数的核心语句
        return true;  //返回true
    }
    else
        return false;
}

4、lasercallback()

laserCallback()函数功能：订阅激光数据，并将其转换到目标坐标系下，然后调用updateMap()函数更新地图。其中，调用了addscan(），updatemap()函数。
这里说明一下为什么要将地图坐标系和里程计坐标系都转换到基坐标系，因为地图坐标系是基坐标系的子坐标系，里程计坐标系是相对于于基坐标系的，将两者都转换到基坐标系下，就可以知道两者的关系了 。 这就相当于你有1英镑，你弟弟有1美元，你两不知道谁的钱多。这时候把你两的钱都换成人民币，就一目了然了。

void MySlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)  
{
    static ros::Time last_map_update(0,0);    //记录上一次地图更新的时间 

    if(!got_first_scan_)   //如果不是第一次接收到激光数据                   
    {
        if(!initMapper(*scan))  //如果初始化失败，则退出            
            return;
        got_first_scan_ = true;     //设置got_first_scan_为true，表示已经获取到了第一帧数据           
    }

    GMapping::OrientedPoint odom_pose;  //定义里程计位姿信息           
    if(addScan(*scan, odom_pose))       //如果添加成功，则更新地图
    {
        std::cout<<"addScan finish"<<std::endl;
           // 发布坐标位姿信息  ，OrientedPoint相对于记录机器人位姿信息的一个类  GMapping::GridSlamProcessor* gsp_
        GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;  //获取最佳粒子的位姿信息
         //这里表示map到base的变换矩阵，即地图坐标系到基坐标系的变换矩阵，另一个表示雷达到地图的变换矩阵
        tf::Transform map_to_base  = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), 
                                                    tf::Vector3(mpose.x, mpose.y, 0.0));   //将刚刚获取的最佳粒子的位姿信息转换为tf::Transform类型的变换矩阵 ，map_to_base表示地图坐标系到基坐标系的变换矩阵  为什么要加0.0？因为地图坐标系的z轴坐标是0.0，而不是雷达坐标系的z轴坐标，所以要加0.0  为什么要转换为基坐标系？因为地图坐标系是基坐标系的子坐标系，所以要转换为基坐标系。
        
        tf::Transform odom_to_base = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), 
                                                    tf::Vector3(odom_pose.x, odom_pose.y, 0.0));  //将里程计坐标转换到基座标系 为什么要转换到基座标系？因为里程计坐标系是相对于基座标系的，而地图坐标系是相对于基座标系的，所以要转换到基座标系。

        map_to_odom_mutex_.lock();  //锁住线程
        map_to_odom_ = map_to_base * (odom_to_base.inverse());  //计算map到odom的变换矩阵，即地图坐标系到里程计坐标系的变换矩阵
        map_to_odom_mutex_.unlock();//解锁线程
          //如果当前激光雷达时间与上一帧时间超过更新阈值或者没有地图，更新地图
        if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
        {
            updateMap(*scan);   //更新地图
            last_map_update = scan->header.stamp;  //更新上一帧时间戳
            std::cout<<"Updated the map"<<std::endl;
        }
    }
    else
    {
        ROS_DEBUG("cannot process scan");
    }
}

5、publishloop()

publishLoop函数功能：周期性发布发布map->odom的转换关系

void MySlamGMapping::publishLoop(double transform_publish_period)   //发布地图的周期
{
    if(transform_publish_period == 0)   //如果发布周期为0，则不发布
        return;

    ros::Rate r(1.0 / transform_publish_period);   //每秒发布一次，每隔transform_publish_period秒发布一次
    while(ros::ok())
    {
        publishTransform();   //发布地图的变换信息
        r.sleep();   //睡眠一段时间
    }
}

今天就先分析这么多，下回再继续分析。