GMapping代码解析[通俗易懂]

全栈程序员站长

发布于 2022-07-02 17:10:54

5660

发布于 2022-07-02 17:10:54

文章被收录于专栏：全栈程序员必看

大家好，又见面了，我是你们的朋友全栈君。

前言：

最近正好用到GMapping，需要改代码，但看过也总是在忘，那干脆写篇博客记录下来同时也可以帮助想要了解GMapping代码的同学。

代码的入口依然是main函数，但GMapping代码中由很多是没有用的，所以并不需要挨个看，可以说代码的作者代码能力挺强但代码风格却是不敢恭维。这里就不带大家挨个文件度代码，只是对几个主要的函数进行介绍。

在看代码前，读者最好选择一个IDE来看代码，因为会涉及大量的跳转，如果只是手动去找的话就太累了。yi

main.cpp

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");

  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();

  return(0);
}

代码的入口，主要作用就是生成了一个类对象，类对象调用成员函数startLiveSlam()。说明一下ros里面基础知识这里就不过多介绍，读者自学一下ros就行。

startLiveSlam()

/*订阅一些主题 发布一些主题*/
void SlamGMapping::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);

    //订阅激光数据 同时和odom_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));

    std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
 

    /*发布转换关系的线程*/
    transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

这个函数的功能就是发布发布四个消息，订阅一个话题，会涉及boost库的一些知识，网上都有搜一下就行。下一步进行回调函数laserCallback中。

laserCallback（）

void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{

    laser_count_++;
    if ((laser_count_ % throttle_scans_) != 0)//throttle_scans_实现降频，如果激光频率较高而
        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_)
    {
        if(!initMapper(*scan))//第一帧激光数据时执行初始化
            return;
        got_first_scan_ = true;
    }

    GMapping::OrientedPoint odom_pose;
    if(addScan(*scan, odom_pose))//初始化完后，再有激光数据时执行addScan函数
    {
        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.theta);
        ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);

        tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
        tf::Transform odom_to_laser = 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_ = (odom_to_laser * laser_to_map).inverse();
        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;
            ROS_DEBUG("Updated the map");
        }
    }
    else
        ROS_DEBUG("cannot process scan");
}

未完待续

发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/148589.html原文链接：https://javaforall.cn

本文参与腾讯云自媒体同步曝光计划，分享自作者个人站点/博客。

如有侵权请联系 cloudcommunity@tencent.com 删除

java