devbox
秋刀鱼在做梦
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ROS之 Gmapping源码解析（第一部分）

作者：秋刀鱼在做梦 | 2024-07-10 09:37:49

赞

踩

gmapping源码

参考：

https://blog.csdn.net/roadseek_zw/article/details/53316177
https://blog.csdn.net/liuyanpeng12333/article/details/81946841
https://blog.csdn.net/weixin_42232742/article/details/82427801
https://blog.csdn.net/u013019296/article/details/78577268
https://blog.csdn.net/weixin_40863346/article/details/88870663
https://blog.csdn.net/weixin_42048023/article/details/85620544
https://cxx0822.github.io/2020/05/05/gmapping-suan-fa-yuan-li-ji-yuan-dai-ma-jie-xi/
https://www.cnblogs.com/xgth/p/13991651.html
https://blog.csdn.net/c417469898/article/details/106566148
https://zhuanlan.zhihu.com/p/85946393

version

2021年12月19日 更新第一部分

  这篇博客以程序运行、调用顺序角度对函数进行分析 理解，第一次接触难免有错，欢迎指正。
  首先唠叨两句，Gmapping 是目前广泛运用的建图算法之一，至于如何建图其实操作比较简单，配置完成参数后直接运行 gmapping.launch（网上一搜一箩筐）加载参数并运行 gmapping 节点，然后通过手柄 joystick_drivers 或者键盘 turtlebot_teleop 等控制机器人移动建立地图。它主要是利用粒子滤波原理进行实时定位再利用固定路径下的栅格地图建图方法建立占用栅格地图。至于参数配置在后期代码分析中会提到。
  其次说一下 slam_gmapping 包与 openslam_gmapping 包的关系，slam_gmapping 是 openslam_gmapping 在 ros 下的二次封装，你可以直接用这个包，而真正的核心代码实现都在 openslam_gmapping 里面。如果使用 apt 工具安装的是 ros-<distro>-desktop-full 版本，那么openslam_gmapping 会自动安装到/opt/ros/<distro>/目录中，但其中不包含源码，无法查看 openslam 中函数定义。因此可以手动下载源码 ，手动进行编译、source，方便使用编辑器进行调用查看。
  最后开始查看 gmapping 源码，从 CMakeLists.txt 中 add_executable(slam_gmapping src/main.cpp src/slam_gmapping.cpp) 可以知道 slam_gmapping.cpp 和 main.cpp 的源文件生成的该节点。因此从 init ros 的 main 函数开始读起。
在这里插入图片描述

1. 进入代码，先用 understand 软件看看 main 函数主要代码的调用关系。
在这里插入图片描述
  由上图调用关系可以明显看出 main 函数直接调用三个函数：SlamGMaping 构造函数、startLiveSlam 函数、~SlamGMapping 析构函数，主要功能的实现是在 startLiveSlam 部分。析构函数不做说明，接下来会对构造函数和 startLiveSlam 函数源码做详细分析。

2. 从 main 函数开始读起

int main(int argc, char** argv)
{
    ros::init(argc, argv, "slam_gmapping");
    SlamGMapping gn;    // code 1_1
    gn.startLiveSlam(); // code 1_2

    ros::spin();
    return(0);
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

2.1 SlamGMapping gn; (code 1_1)

  首先，创建 SlamGMapping 类对象 gn，程序运行时首先运行 SlamGMapping 类构造函数；

SlamGMapping();
SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh);
SlamGMapping(unsigned long int seed, unsigned long int max_duration_buffer);

  • 1
  • 2
  • 3

  其中，构造函数被重载，这里不做深入研究，每个重载均有调用成员函数 init()。本程序调用无参构造函数 SlamGMapping();

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)   // Part A
{
    seed_ = time(NULL);  // Part B
    init();  // Part C
}

  • 1
  • 2
  • 3
  • 4
  • 5

  • Part A. 初始化成员变量列表：

    • 从地图到世界坐标 map_to_odom_(tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Point(0, 0, 0))) 从 RPY–>quaternion
    • laser_count_(0) 激光雷达数据计数，用于限制处理激光的间隔 Step
    • private_nh_("~") 私有句柄
    • scan_filter_sub_(NULL) 激光雷达订阅
    • scan_filter_(NULL) 激光雷达滤波器
    • transform_thread_(NULL) tf 变换线程

  • Part B. seed_ = time(NULL); 随机种子 供后期高斯采样使用，time(NULL) 作为 srand 函数的参数，产生不同的随机数序列。

    // Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);

double sampleGaussian(double sigma, unsigned long int S) {
/*
    static gsl_rng * r = NULL;
    if(r==NULL) {
    gsl_rng_env_setup();
    r = gsl_rng_alloc (gsl_rng_default);
    }
*/
    if (S!=0) {
        //gsl_rng_set(r, S);
        srand48(S);
    }
    if (sigma==0)
        return 0;
    //return gsl_ran_gaussian (r,sigma);
    return pf_ran_gaussian (sigma);
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20

  • Part C. init();

    • gsp_ = new GMapping::GridSlamProcessor(); GMapping 为命名空间，GridSlamProcessor 为类名称，构建 gsp_ 类对象，在这里可以初始化类参数。重要的参数 period_= 5.0 ，其余参数赋值，initMapper 中会重新赋值。

      // openslam_gmapping/gridfastslam/gridslamprocessor.cpp GridSlamProcessor构造函数
GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout) {  
    period_ = 5.0; 
    m_obsSigmaGain=1;
    m_resampleThreshold=0.5;
    m_minimumScore=0.;
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7

    • tfB_ = new tf::TransformBroadcaster(); 定义 tfB 变量，用于发布 map–>odom tf 关系

    • got_first_scan_ = false; 初始化成员变量为 false，用于地图初始化限制，直到获取到第一帧激光数据才进行地图初始化

    • gmapping 参数获取以及初始化如下

    <launch>
     <arg name="scan_topic"  default="scan" /> <!-- laser的topic名称，与自己的激光topic对应 -->
     <arg name="base_frame"  default="base_footprint"/> <!-- 机器人的坐标系 -->
     <arg name="odom_frame"  default="odom"/> <!-- 世界坐标 -->
   
     <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> <!-- 启动slam的节点 -->
       <param name="base_frame" value="$(arg base_frame)"/>
       <param name="odom_frame" value="$(arg odom_frame)"/>
       <param name="map_update_interval" value="5"/> <!-- 地图更新的一个间隔，两次scanmatch的间隔，地图更新也受scan mach的影响，如果scan match没有成功的话，是不会更新地图的 -->
   
     <!-- 1. Laser Parameters scanMatchingParameters setMotionModelParameters -->
       <param name="maxUrange" value="4.0"/> <!-- maximum range of the laser scanner that is used for map building (default: same as maxRange) -->
       <param name="maxRange" value="5.0"/>
       <param name="sigma" value="0.05"/> <!-- 贪心终点匹配使用的sigma. standard deviation for the scan matching process (cell) -->
       <param name="kernelSize" value="1"/> <!-- 内核中要查找一个对应关系 search window for the scan matching process -->
       <param name="lstep" value="0.05"/> <!-- optimize机器人平移的初始值（距离） initial search step for scan matching (linear) -->
       <param name="astep" value="0.05"/> <!-- optimize机器人旋转的初始值（角度） initial search step for scan matching (angular) -->
       <param name="iterations" value="5"/> <!-- icp的迭代次数  number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations) -->
       <param name="lsigma" value="0.075"/> <!-- standard deviation for the scan matching process (single laser beam) -->
       <param name="ogain" value="3.0"/> <!-- 在评估可能性时使用的增益，用于平滑重采样效果 gain for smoothing the likelihood -->
       <param name="lskip" value="0"/> <!-- 为0,表示所有的激光都处理，尽可能为零，如果计算压力过大，可以改成1 -->
   
       <param name="minimumScore" value="30"/> <!-- 很重要，判断scanmatch是否成功的阈值，过高的话会使scanmatch失败，从而影响地图更新速率. minimum score for considering the outcome of the scanmatching good. Can avoid 'jumping' pose estimates in large open spaces when using laser scanners with limited range (e.g. 5m). (0 = default. Scores go up to 600+, try 50 for example when experiencing 'jumping' estimate issues) -->
   
     <!-- 2. Motion Model Parameters (all standard deviations of a gaussian noise model) -->
       <param name="srr" value="0.01"/> <!-- 以下四个参数是运动模型的噪声参数  作为平移函数的平移误差（rho / rho）  linear noise component (x and y) -->
       <param name="srt" value="0.02"/> <!-- 作为旋转的函数的平移误差（rho / theta） linear -> angular noise component -->
       <param name="str" value="0.01"/> <!-- 作为平移函数的旋转中的测量误差（θ/ rho） angular -> linear noise component -->
       <param name="stt" value="0.02"/> <!-- 作为旋转的函数的旋转中的测量误差（θ/θ）  angular noise component (theta) -->
   
     <!-- 3. Others -->
       <param name="linearUpdate" value="0.5"/> <!-- 机器人移动linearUpdate距离，进行scanmatch.  the robot only processes new measurements if the robot has moved at least this many meters -->
       <param name="angularUpdate" value="0.436"/> <!-- 机器人旋转angularUpdate角度，进行scanmatch. the robot only processes new measurements if the robot has turned at least this many rads -->
       <param name="resampleThreshold" value="0.5"/>  <!-- threshold at which the particles get resampled. Higher means more frequent resampling. -->
       <param name="particles" value="8"/> <!-- 很重要，粒子个数  (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled -->
     
       <param name="temporalUpdate" value="-1.0"/>
   
     <!--
       <param name="xmin" value="-50.0"/>
       <param name="ymin" value="-50.0"/>
       <param name="xmax" value="50.0"/>
       <param name="ymax" value="50.0"/>
       make the starting size small for the benefit of the Android client's memory...
     -->
   
     <!-- 4. Initial map dimensions and resolution -->
       <param name="xmin" value="-1.0"/> <!-- map初始化的大小 -->
       <param name="ymin" value="-1.0"/>
       <param name="xmax" value="1.0"/>
       <param name="ymax" value="1.0"/>
       <param name="delta" value="0.05"/> <!-- 地图的分辨率 -->
   
     <!-- 5. Likelihood sampling (used in scan matching) -->
       <param name="llsamplerange" value="0.01"/>  <!-- 可能性的平移采样范围 linear range -->
       <param name="llsamplestep" value="0.01"/>   <!-- 可能性的平移采样步骤 linear step size -->
       <param name="lasamplerange" value="0.005"/> <!-- 角度采样范围的可能性 angular range -->
       <param name="lasamplestep" value="0.005"/>  <!-- 角度采样步骤的可能性 angular step size -->
       <remap from="scan" to="$(arg scan_topic)"/>
     </node>
   </launch>

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61

需要关注的两个参数：
  particles (int, default: 30) gmapping 算法中的粒子数，因为 gmapping 使用的是粒子滤波算法，粒子在不断地迭代更新，所以选取一个合适的粒子数可以让算法在保证比较准确的同时有较高的速度。
  minimumScore (float, default: 0.0) 最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，越高说明对激光匹配算法的要求越高，激光的匹配也越容易失败而转去使用里程计数据，而设的太低又会使地图中出现大量噪声，所以需要权衡调整。

2.2 gn.startLiveSlam(); (code 1_2)

void SlamGMapping::startLiveSlam()
{
    // Part B. -------------------------------------------------------------------
    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);

    // Part C. -------------------------------------------------------------------
    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));

    // Part A. -------------------------------------------------------------------
    transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

  整个函数如上，这个函数发布的话题有信息熵、地图以及地图的详细栅格信息（Metadata），订阅的话题有 scan。另外，创建了一个新线程 transform_thread_，在固定时刻发布 odom 到 map 的 tf 变换，也在不停地纠正里程计提供的位置。接下来分段解析。

  • Part A. 先拿简单的 transform_thread_ 说起：

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

    • 1

  使用 boost::bind 创建线程调用成员函数 publishLoop， 从而定频调用函数 publishTransform 发布 map --> odom 的 tf 变换，时间间隔 transform_publish_period_ 默认 0.05s。添加 boost::mutex 线程互斥锁防止共享 tf 数据的读写问题。map–>odom 的 tf 实时修正更新是在 laserCallback 中进行。map_to_odom_ = (odom_to_laser * laser_to_map).inverse();

  • Part B. 发布话题服务

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

    • 1
    • 2
    • 3
    • 4
    • 其中 entropy_publisher_ 和 ss_ 不知道用来做什么
    • sst_, sstm_ 为发布 map topic
    • ss_ 通过 dynamic_map 服务获取地图信息

  • Part C. 激光数据处理

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

    • 1
    • 2
    • 3

      最麻烦也是最重要的部分，tf::MessageFilter 可以订阅任何的 ROS 消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（一般在回调函数中处理），说白了就是激光消息订阅+坐标转换处理（回调函数 laserCallback）。当 message_filters::Subscriber 的消息能够由 tf 转换到目标坐标系时，调用回调函数，回调函数由 tf::MessageFilter::registerCallback() 进行注册。

    • tf::MessageFilter结构：
      • 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
      • 用消息的名称来初始化 message_filters::Subscriber
      • 用 tf、message_filters::Subscriber、目标坐标系来初始化 tf::MessageFilter
      • 给 tf::MessageFilter 注册 callback。
      • 编写 callback，并在回调中完成坐标转换。至此完成消息订阅+坐标转换。

2.3 laserCallback

  接下来介绍核心部分 laserCallback 函数。

void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    // Part A. -------------------------------------------------------------------
    laser_count_++;
    if ((laser_count_ % throttle_scans_) != 0) 
        return; 
    /* 
     * 以上三行 laser_count_++ 进行 laser data 的计数，自增处理；
     * throttle_scans_ 为每隔多少次处理一次激光雷达数据（设大跳过更多的扫描数据），默认为1（全处理）。
     * 此条件语句为了限制激光数据处理频率，起到限流作用。
     */

    // Part B. -------------------------------------------------------------------
    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_)
    {
    /* 
     * 首次获取 scan 数据，调用 initMapper 函数进行一些重要参数的初始化，将 slam 里的参数传递给 openslam，
     * 设定坐标系，坐标原点，以及采样函数随机种子的初始化，等等，
     * 里面还调用了 GridSlamProcessor::init，这个初始化了粒子数，子地图大小 
     */
        if(!initMapper(*scan))
            return;
        got_first_scan_ = true;
    }

    // Part C. -------------------------------------------------------------------
    GMapping::OrientedPoint odom_pose;

    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.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");
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55

  • Part A. 激光数据限流

    laser_count_++;
if ((laser_count_ % throttle_scans_) != 0) 
    return; 

    • 1
    • 2
    • 3

  前三行如上，laser_count_++ 进行 laser data 的自增计数；throttle_scans_ 为每隔多少次处理一次激光雷达数据（设大跳过更多的扫描数据），默认为1（全处理）。此条件语句在此为了限制激光数据处理频率，起到限流作用，为了使激光雷达数据每隔若干帧进行计算。

  • Part B. initMapper
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_)
  {
      /* 
       * 首次获取 scan 数据，调用 initMapper 函数进行一些重要参数的初始化，将 slam 里的参数传递给 openslam，
       * 设定坐标系，坐标原点，以及采样函数随机种子的初始化，等等，
       * 里面还调用了 GridSlamProcessor::init，这个初始化了粒子数，子地图大小 
       */
      if (!initMapper(*scan))
          return;
      got_first_scan_ = true;
  }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

  当获取到第一帧激光数据后初始化 mapper，其中包括一些重要参数的初始化等；具体 initMapper 函数如下：

bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{
    // Part B1. -------------------------------------------------------------------
    laser_frame_ = scan.header.frame_id;

    // Get the laser's pose, relative to base. laer在base_link下的位置
    tf::Stamped<tf::Pose> ident;
    tf::Stamped<tf::Transform> laser_pose;
    ident.setIdentity();
    ident.frame_id_ = laser_frame_;
    ident.stamp_ = scan.header.stamp;
    try {
        tf_.transformPose(base_frame_, ident, laser_pose);
        // ROS_INFO("init map. laser_pose: %f, %f, %f", laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), laser_pose.getOrigin().z());
    } catch(tf::TransformException e) {
        ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what());
        return false;
    }

    // Part B2. ------------------------------------------------------------------
    // create a point 1m above the laser position and transform it into the laser-frame
    tf::Vector3 v;
    v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
    tf::Stamped<tf::Vector3> up(v, scan.header.stamp, base_frame_);

    try {
        tf_.transformPoint(laser_frame_, up, up);
        ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
    } catch(tf::TransformException& e) {
        ROS_WARN("Unable to determine orientation of laser: %s", e.what());
        return false;
    }

    // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
    if (fabs(fabs(up.z()) - 1) > 0.001) {
        ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.z());
        return false;
    }

    gsp_laser_beam_count_ = scan.ranges.size();
    double angle_center = (scan.angle_min + scan.angle_max)/2;

    if (up.z() > 0) {
        do_reverse_range_ = scan.angle_min > scan.angle_max;
        centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center), tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
        ROS_INFO("Laser is mounted upwards.");
    } else {
        do_reverse_range_ = scan.angle_min < scan.angle_max;
        centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center), tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
        ROS_INFO("Laser is mounted upside down.");
    }

    // Compute the angles of the laser from -x to x, basically symmetric and in increasing order
    laser_angles_.resize(scan.ranges.size());

    // Make sure angles are started so that they are centered
    double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
    for(unsigned int i=0; i<scan.ranges.size(); ++i)
    {
        laser_angles_[i]=theta;
        theta += std::fabs(scan.angle_increment);
    }

    ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max, scan.angle_increment);
    ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(), laser_angles_.back(), std::fabs(scan.angle_increment));

    // Part B3. ------------------------------------------------------------------
    GMapping::OrientedPoint gmap_pose(0, 0, 0);

    // setting maxRange and maxUrange here so we can set a reasonable default
    ros::NodeHandle private_nh_("~");
    if(!private_nh_.getParam("maxRange", maxRange_))
        maxRange_ = scan.range_max - 0.01;
    if(!private_nh_.getParam("maxUrange", maxUrange_))
        maxUrange_ = maxRange_;

    // The laser must be called "FLASER".
    // We pass in the absolute value of the computed angle increment, on the
    // assumption that GMapping requires a positive angle increment.  If the
    // actual increment is negative, we'll swap the order of ranges before
    // feeding each scan to GMapping.
    gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(scan.angle_increment), gmap_pose, 0.0, maxRange_);
    ROS_ASSERT(gsp_laser_);

    GMapping::SensorMap smap;
    smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));  // 将”FLASER"写入gsp_laser_
    gsp_->setSensorMap(smap);

    gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
    ROS_ASSERT(gsp_odom_);

    /// @todo Expose setting an initial pose
    GMapping::OrientedPoint initialPose;
    if(!getOdomPose(initialPose, scan.header.stamp))
    {
        ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
        initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
    }

    gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_, lsigma_, ogain_, lskip_);
    gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
    gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
    gsp_->setUpdatePeriod(temporalUpdate_);
    gsp_->setgenerateMap(false);
    gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose);
    gsp_->setllsamplerange(llsamplerange_);
    gsp_->setllsamplestep(llsamplestep_);

    /// @todo Check these calls; in the gmapping gui, they use
    /// llsamplestep and llsamplerange intead of lasamplestep and
    /// lasamplerange.  It was probably a typo, but who knows.
    gsp_->setlasamplerange(lasamplerange_);
    gsp_->setlasamplestep(lasamplestep_);
    gsp_->setminimumScore(minimum_score_);

    // Call the sampling function once to set the seed.
    GMapping::sampleGaussian(1,seed_);
    ROS_INFO("Initialization complete");

    return true;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121

  • Part B1. 通过 tf 获取激光在 base 坐标系下的静态位置信息，通过 ROS_INFO 可以打印比对。

    laser_frame_ = scan.header.frame_id;

// Get the laser's pose, relative to base. laser在base_link下的位置
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();
ident.frame_id_ = laser_frame_;
ident.stamp_ = scan.header.stamp;
try {
    tf_.transformPose(base_frame_, ident, laser_pose);
    // ROS_INFO("init map. laser_pose: %f, %f, %f", laser_pose.getOrigin().x(), laser_pose.getOrigin().y(), laser_pose.getOrigin().z());
} catch(tf::TransformException e) {
    ROS_WARN("Failed to compute laser pose, aborting initialization (%s)", e.what());
    return false;
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15

  • Part B2. 激光安装判断

    // create a point 1m above the laser position and transform it into the laser-frame
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
tf::Stamped<tf::Vector3> up(v, scan.header.stamp, base_frame_);

try {
    tf_.transformPoint(laser_frame_, up, up);
    ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
} catch(tf::TransformException& e) {
    ROS_WARN("Unable to determine orientation of laser: %s", e.what());
    return false;
}

// 判断激光是否水平安装 
// gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
if (fabs(fabs(up.z()) - 1) > 0.001) {
    ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.z());
    return false;
}

// 判断激光安装方向，是z向上还是z向下
gsp_laser_beam_count_ = scan.ranges.size();
double angle_center = (scan.angle_min + scan.angle_max)/2;

if (up.z() > 0) {
    do_reverse_range_ = scan.angle_min > scan.angle_max;
    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center), tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
    ROS_INFO("Laser is mounted upwards.");
} else {
    do_reverse_range_ = scan.angle_min < scan.angle_max;
    centered_laser_pose_ = tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(M_PI,0,-angle_center), tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
    ROS_INFO("Laser is mounted upside down.");
}

// Compute the angles of the laser from -x to x, basically symmetric and in increasing order 激光角度数据格式处理 (-x, x) 对称，递增
laser_angles_.resize(scan.ranges.size());

// Make sure angles are started so that they are centered
double theta = - std::fabs(scan.angle_min - scan.angle_max)/2;
for(unsigned int i=0; i<scan.ranges.size(); ++i)
{
    laser_angles_[i]=theta;
    theta += std::fabs(scan.angle_increment);
}

ROS_DEBUG("Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", scan.angle_min, scan.angle_max, scan.angle_increment);
ROS_DEBUG("Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", laser_angles_.front(), laser_angles_.back(), std::fabs(scan.angle_increment));

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47

      在 base 坐标系原点创建一个新点位，点位 z 轴值为激光高度+1m，计算此点位位置在激光坐标系下坐标。创建新点位的原因一是 gmapping 不考虑横滚、俯仰的问题，所以需要检查实际安装的激光是否水平，与 base 坐标系是否不存在横滚、俯仰角度。若水平安装，其实在 base 坐标系下激光 z+1，与激光坐标系下+1的值是相等的。原因二是为了判断激光安装方向。

      这段代码激光数据进行简单处理，判断激光安装姿态（只考虑 yaw，不考虑 roll pitch，所以需要水平安装），判断了 Z 轴方向（up or down），并将激光数据搞成规则的最小、最大、中间开始的数据。其中重要的是 centered_laser_pose_ 变量，后期 getOdomPose 时会使用其转换 laser 中心位置到 odom 坐标系下。

  • Part B3. Slam参数初始化

    // 初始化位置，后期 smap 会赋值给 m_pose (rangesensor)
GMapping::OrientedPoint gmap_pose(0, 0, 0);

// setting maxRange and maxUrange here so we can set a reasonable default
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("maxRange", maxRange_))
    maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
    maxUrange_ = maxRange_;

// The laser must be called "FLASER".
// We pass in the absolute value of the computed angle increment, on the
// assumption that GMapping requires a positive angle increment.  If the
// actual increment is negative, we'll swap the order of ranges before
// feeding each scan to GMapping.
// 激光name规定"FLASER"，GridSlamProcessor::setSensorMap中会去find name，如果不是"FLASER"会报错 "Attempting to load the new carmen log format"
gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(scan.angle_increment), gmap_pose, 0.0, maxRange_);
ROS_ASSERT(gsp_laser_);

// GridSlamProcessor::setSensorMap  把Beam中所有的angle pose 取出，调用ScanMatcher::setLaserParameters 函数 memcpy 给 m_laserAngles, pose赋值给m_laserPose
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));  // 将”FLASER"写入gsp_laser_
gsp_->setSensorMap(smap);

gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
ROS_ASSERT(gsp_odom_);

/// @todo Expose setting an initial pose
GMapping::OrientedPoint initialPose;  // 激光在odom坐标系下的 GMapping::OrientedPoint 类型的位姿(x, y, yaw) 位置获取失败就对初始位置置零
if(!getOdomPose(initialPose, scan.header.stamp))
{
    ROS_WARN("Unable to determine inital pose of laser! Starting point will be set to zero.");
    initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
}

gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, kernelSize_, lstep_, astep_, iterations_, lsigma_, ogain_, lskip_);
gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
gsp_->setUpdatePeriod(temporalUpdate_);
gsp_->setgenerateMap(false);
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose);
gsp_->setllsamplerange(llsamplerange_);
gsp_->setllsamplestep(llsamplestep_);

/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange.  It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);
gsp_->setlasamplestep(lasamplestep_);
gsp_->setminimumScore(minimum_score_);

// Call the sampling function once to set the seed.
GMapping::sampleGaussian(1,seed_);
ROS_INFO("Initialization complete");

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    gsp_laser_ = new GMapping::RangeSensor("FLASER",
                                       gsp_laser_beam_count_,
                                       fabs(scan.angle_increment),
                                       gmap_pose,
                                       0.0,
                                       maxRange_);

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

      RangeSensor 位于 openslam_gmapping/…/sensor_range/rangesensor.h 中，新建对象 gsp_laser_ 并初始化，RangeSensor 为派生类 基类为 Sensor，派生类中构建了公有结构体 Beam 包含 pose, span, maxRange, s, c 变量，构造函数 RangeSensor 中将每一帧中 beam 上的数据封装为 RangeSensor::Beam 类型，实现测距模型的初始化。源码如下：

    RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), m_pose(position), m_beams(beams_num) {
    double angle=-.5*res*beams_num;
    for (unsigned int i=0; i<beams_num; i++, angle+=res) {
        RangeSensor::Beam& beam(m_beams[i]);
        beam.span=span;
        beam.pose.x=0;
        beam.pose.y=0;
        beam.pose.theta=angle;
        beam.maxRange=maxrange;
    }
    newFormat=0;
    updateBeamsLookup();
}

void RangeSensor::updateBeamsLookup() {
    for (unsigned int i=0; i<m_beams.size(); i++) {
        RangeSensor::Beam& beam(m_beams[i]);
        beam.s=sin(m_beams[i].pose.theta);
        beam.c=cos(m_beams[i].pose.theta);
    }
}

// 每个波束的结构体
struct Beam {
    OrientedPoint pose;	//pose relative to the center of the sensor
    double span;	//spam=0 indicates a line-like beam
    double maxRange;	//maximum range of the sensor
    double s,c;		//sinus and cosinus of the beam (optimization);
};	

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_,
                            kernelSize_, lstep_, astep_, iterations_,
                            lsigma_, ogain_, lskip_);

gsp_->setMotionModelParameters(srr_, srt_, str_, stt_);  // 运动模型参数
gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);  // 设置更新距离
gsp_->setUpdatePeriod(temporalUpdate_); // 在h函数中 赋值period
gsp_->setgenerateMap(false);  // false: 只建立障碍部分； true: 完整的建立地图，包括障碍和空地 
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
                              delta_, initialPose); // 初始化粒子滤波器
gsp_->setllsamplerange(llsamplerange_);  // gridslamprocessor.h accessor methods 
gsp_->setllsamplestep(llsamplestep_);   // gridslamprocessor.h accessor methods 
/// @todo Check these calls; in the gmapping gui, they use
/// llsamplestep and llsamplerange intead of lasamplestep and
/// lasamplerange.  It was probably a typo, but who knows.
gsp_->setlasamplerange(lasamplerange_);  // gridslamprocessor.h accessor methods 
gsp_->setlasamplestep(lasamplestep_);  // gridslamprocessor.h accessor methods 
gsp_->setminimumScore(minimum_score_);  // gridslamprocessor.h accessor methods 

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    void ScanMatcher::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations,  double likelihoodSigma, unsigned int likelihoodSkip){	
    m_usableRange=urange;
    m_laserMaxRange=range;
    m_kernelSize=kernsize;
    m_optLinearDelta=lopt;
    m_optAngularDelta=aopt;
    m_optRecursiveIterations=iterations;
    m_gaussianSigma=sigma;
    m_likelihoodSigma=likelihoodSigma;
    m_likelihoodSkip=likelihoodSkip;
}

void GridSlamProcessor::setMotionModelParameters (double srr, double srt, double str, double stt){
    m_motionModel.srr=srr;
    m_motionModel.srt=srt;
    m_motionModel.str=str;
    m_motionModel.stt=stt;
}

void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){
    m_linearThresholdDistance=linear; 
    m_angularThresholdDistance=angular;
    m_resampleThreshold=resampleThreshold;	
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    // 粒子滤波器初始化 初始化了最初始地图的大小、初始位置以及初始粒子数
void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
    m_xmin=xmin;
    m_ymin=ymin;
    m_xmax=xmax;
    m_ymax=ymax;
    m_delta=delta;
    
    m_particles.clear(); // m_particles 定义见下文
    TNode* node=new TNode(initialPose, 0, 0, 0);
    ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
    for (unsigned int i=0; i<size; i++){
        m_particles.push_back(Particle(lmap));
        m_particles.back().pose=initialPose;
        m_particles.back().previousPose=initialPose;
        m_particles.back().setWeight(0);
        m_particles.back().previousIndex=0;
      
		// this is not needed
		// m_particles.back().node=new TNode(initialPose, 0, node, 0);

		// we use the root directly
		m_particles.back().node= node;
    }
    m_neff=(double)size;
    m_count=0;
    m_readingCount=0;
    m_linearDistance=m_angularDistance=0;
}

-----------
// gridslamprocessor.h

typedef std::vector<Particle> ParticleVector;
/**the particles*/
ParticleVector m_particles;

/**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/
struct Particle{
    /**constructs a particle, given a map
	 @param map: the particle map
     */
    Particle(const ScanMatcherMap& map);

    /** @returns the weight of a particle */
    inline operator double() const {return weight;}
    /** @returns the pose of a particle */
    inline operator OrientedPoint() const {return pose;}
    /** sets the weight of a particle
	 @param w the weight
     */
    inline void setWeight(double w) {weight=w;}
    /** The map */
    ScanMatcherMap map;
    /** The pose of the robot */
    OrientedPoint pose;
    /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */
    OrientedPoint previousPose;
    /** The weight of the particle */
    double weight;
    /** The cumulative weight of the particle */
    double weightSum;
    double gweight;
    /** The index of the previous particle in the trajectory tree */
    int previousIndex;
    /** Entry to the trajectory tree */
    TNode* node; 
};

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68

      总的来说， initMapper 这部分代码是做初始化过程。首先判断激光 Z 轴的安装方向（up or down），只考虑水平安装情况，然后初始化 gsp_laser_、 smap、 gsp_odom_，定义的 GMapping::GridSlamProcessor* gsp_ 对象用于获取 gmapping 参数，导入 fastslam 和 scanMatcher，初始化粒子滤波器。

  • Part C.

    GMapping::OrientedPoint odom_pose;

// Part C1. ------------------------------------------------------------------
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.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();

    // Part C2. ------------------------------------------------------------------
    // 每超过map_update_interval_时间就要调用updateMap更新地图函数，周期性更新一次地图
    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");

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29

  • Part C1 addScan

    bool SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{
    // 获取与激光雷达时间戳相对应的机器人里程计位姿
    if(!getOdomPose(gmap_pose, scan.header.stamp))
        return false;
    // 判断激光束数量
    if(scan.ranges.size() != gsp_laser_beam_count_)
        return false;

    // GMapping wants an array of doubles...
    double* ranges_double = new double[scan.ranges.size()];
    // If the angle increment is negative, we have to invert the order of the readings.
    // 如果激光是反着装的，则激光数据存储的顺序需要反过来
    if (do_reverse_range_) {
        ROS_DEBUG("Inverting scan");
        int num_ranges = scan.ranges.size();
        for(int i=0; i < num_ranges; i++) {
            // Must filter out short readings, because the mapper won't
            if(scan.ranges[num_ranges - i - 1] < scan.range_min)
                ranges_double[i] = (double)scan.range_max;
            else
                ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];
        }
    } else {
        for(unsigned int i=0; i < scan.ranges.size(); i++) {
            // Must filter out short readings, because the mapper won't
            if(scan.ranges[i] < scan.range_min)
                ranges_double[i] = (double)scan.range_max;
            else
                ranges_double[i] = (double)scan.ranges[i];
        }
    }
    // 将ROS的激光雷达的数据信息转换为GMapping算法中的数据格式
    GMapping::RangeReading reading(scan.ranges.size(),
                                   ranges_double,
                                   gsp_laser_,
                                   scan.header.stamp.toSec());

    // ...but it deep copies them in RangeReading constructor, so we don't
    // need to keep our array around.
    delete[] ranges_double;
    // 设置和激光雷达数据时间戳相匹配的机器人位姿
    reading.setPose(gmap_pose);

    /*
    ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",
              scan.header.stamp.toSec(),
              gmap_pose.x,
              gmap_pose.y,
              gmap_pose.theta);
    */
    ROS_DEBUG("processing scan");
    // 调用GMapping算法进行处理
    return gsp_->processScan(reading);
}

bool SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{
    // Get the pose of the centered laser at the right time
    centered_laser_pose_.stamp_ = t;
    // Get the laser's pose that is centered
    tf::Stamped<tf::Transform> odom_pose;
    try {
        tf_.transformPose(odom_frame_, centered_laser_pose_, odom_pose);
    } catch(tf::TransformException e) {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }
    double yaw = tf::getYaw(odom_pose.getRotation());
    gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), yaw);
    return true;
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61
    • 62
    • 63
    • 64
    • 65
    • 66
    • 67
    • 68
    • 69
    • 70
    • 71
    • 72

    接下来进入 Gmapping的主角部分。

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号