GMapping代码解析[通俗易懂]

GMapping代码解析[通俗易懂]前言:   最近正好用到GMapping,需要改代码,但看过也总是在忘,那干脆写篇博客记录下来同时也可以帮助想要了解GMapping代码的同学。   代码的入口依然是main函数,但GMapping代码中由很多是没有用的,所以并不需要挨个看,可以说代码的作者代码能力挺强但代码风格却是不敢恭维。这里就不带大家挨个文件度代码,只是对几个主要的函数进行介绍。   …

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

前言:

      最近正好 用到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");
}

未完待续

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。

发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/148589.html原文链接:https://javaforall.cn

【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛

【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...

(0)


相关推荐

  • mysql函数

    mysql函数

    2021年10月15日
  • idea安装下载「建议收藏」

    idea安装下载「建议收藏」idea安装下载

  • 视频采用的压缩编码_视频压缩编码存在的冗余信息

    视频采用的压缩编码_视频压缩编码存在的冗余信息1、什么是H.261编码协议        答:H.261是最早出现的视频编码建议,它采用的算法结合了可减少时间冗余的帧间预测和可减少空间冗余的DCT变换的混合编码方法,其输出码率是p×64kbit/s。p取值较小时,只能传清晰度不太高的图像,适合于面对面的电视电话;p取值较大时(如p>6),可以传输清晰度较好的会议电视图像。该标准主要针对ISDN电话线的视频会议,可视电话等,ISDN的

    2022年10月30日
  • 进程调度时间片轮转例题_进程调度算法java

    进程调度时间片轮转例题_进程调度算法java一、实验目的(1)加深对进程的理解(2)理解进程控制块的结构(3)理解进程运行的并发性(4)掌握时间片轮转法进程调度算法二、实验原理(1)建立进程控制块(2)设计两个链队列,分别表示就绪队列和完成队列(3)用户输入进程标识符,进程到达时间,进程所需的时间,申请空间存放进程,PCB信息。(

    2022年10月28日
  • 【AI视野·今日NLP 自然语言处理论文速览 第十七期】Thu, 1 Jul 2021[通俗易懂]

    AI视野·今日CS.NLP自然语言处理论文速览Thu,1Jul2021Totally28papers????上期速览✈更多精彩请移步主页DailyComputationandLanguagePapersOnthePowerofSaturatedTransformers:AViewfromCircuitComplexityAuthorsWilliamMerrill,YoavGoldberg,RoySchwartz,NoahA.

  • 阿里云服务器搭建及项目部署过程—小白篇

    阿里云服务器搭建及项目部署过程—小白篇最近学习了前后端的相关技术,就想租一个服务器试一下,玩一玩,简单了解了一下阿里云的服务器,简单介绍一下:一:什么是云服务器ECS是阿里云产品体系中,最基础的计算服务,通常用作应用程序的运行环境,最重要的特点是弹性。二:基础运行环境用户的应用程序运行在实例的操作系统上三:特点弹性:容量不够可以直接在云服务器上扩展配置,只要直接补差价成本:0运维,支持包年包月或按量计费…

发表回复

您的电子邮箱地址不会被公开。

关注全栈程序员社区公众号