之前博文《ROS学习笔记之——gmapping与amcl 》已经介绍过gmapping与amcl了。本博文详细的看一下gmapping的代码。
本博文仅仅用于本人学习记录。
目录
GMapping是一种基于RBPF的SLAM解决方案。这种解决方案是先估计的机器人运动轨迹，再根据机器人的状态估计环境地图；然后反过来这个环境地图又可以用来更新机器人的运动轨迹。 在算法中的每个粒子都独立的记录了一条可能的机器人轨迹及其对应的环境地图。相当于牺牲了空间（存储）而赢得了时间（计算效率）。但由于gmapping本身每个粒子都会携带自己的地图，其不适用于大场景，由于大场景建图每个粒子都存在很大的地图，这样导致存储容易撑爆。
Gmapping的基本原理
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=index
https://blog.csdn.net/zhao_ke_xue/article/details/108944811
FastSLAM
FastSLAM问题的分解
对于一个SLAM的问题，可以描述为如下的后验概率的问题。Gmapping其实就是，在移动机器人从开机到t时刻一系列传感器测量数据z1:t（/scan）以及一系列控制数据u1:t(/odom)的条件下，同时对地图m、机器人轨迹状态x1:t进行的估计。
而上面的公式，可以分解为下面的形式：
一个是机器人定位问题（确定机器人的轨迹），另一个是已知机器人位姿（轨迹）进行地图构建的问题。
其中p(x1:t | u1:t, z1:t)表示估计机器人的轨迹，p(m|x1:t, z1:t) 表示在已知机器人轨迹和传感器观测数据情况下，进行地图构建的闭式计算。这样 SLAM 问题就分解成两个问题。其中已知机器人位姿的地图构建是个简单问题，所以机器人位姿的估计是一个重点问题。
FastSLAM机器人轨迹估计问题
FastSLAM算法采用粒子滤波来估计机器人的位姿，并且为每一个粒子构建一个地图。所以，每一个粒子都包含了机器人的轨迹和对应的环境地图。 现在我们着重研究一下 p(x1:t | u1:t, z1:t) 估计机器人的轨迹 。 通过使用贝叶斯准则对 p(x1:t | u1:t, z1:t) 进行公式推导如式（3.3）所示 ：
关于上面的推导可以参考
经过上面的公式推导，这里将机器人轨迹估计转化成一个增量估计的问题，用p(x1:t-1 | u1:t-1, z1:t-1) 表示上一时刻的机器人轨迹，用上一时刻的粒子群表示。每一个粒子都用运动学模型p(xt | xt-1, ut)进行状态传播，这样就得到每个粒子对应的预测轨迹 。对于每一个传播之后的粒子，用观测模型p(zt | xt)进行权重计算归一化处理，这样就得到该时刻的机器人轨迹。之后根据估计的轨迹以及观测数据进行地图构建。
每一个粒子，都携带了上一时刻的位姿、权重、地图。通过公式3.3的计算，通过里程计的运动模型，得到了不同粒子预测的位姿。而在观测模型的作用下，得到了该粒子对当前的机器人位姿的估计。然后再通过公式3.2，根据机器人轨迹结合观测的数据，得到了粒子代表的地图。
进而实现了，每个粒子保存一个机器人的轨迹和环境的地图。
Gmapping（FastSLAM2.0）
从上面的FastSLAM中，我们发现其存在两个问题：
- 当环境大或者机器人里程计误差大时，需要更多的粒子才能得到较好的估计，这时将造成内存爆炸；
- 粒子滤波避免不了使用重采样，以确保当前粒子的有效性，然而重采样带来的问题就是粒子耗散，粒子多样性的丢失。
为此，Gmapping就做了improvement
降低粒子的数量
问题由来：每一个粒子都包含自己的栅格地图，对于一个稍微大一点的环境来说，每一个粒子都会占用比较大的内存。如果机器人里程计的误差比较大，即proposal分布（提议分布）跟实际分布相差较大，则需要较多的粒子才能比较好的表示机器人位姿的估计，这样将会造成内存爆炸。
目的：通过降低粒子数量的方法大幅度缓解内存爆炸。
分析：里程计的概率模型比较平滑，是一个比较大的范围，如果对整个范围采样将需要很多的粒子，如果能找到一个位姿优值，在其周围进行小范围采样，这样不就可以降低粒子数量了嘛。
方法一：直接采用极大似然估计的方式，根据粒子的位姿的预测分布和与地图的匹配程度，通过扫描匹配找到粒子的最优位姿参数（由于激光雷达的测量要准确很多，通过其匹配的方差要小很多），就用该位姿参数，直接当做新粒子的位姿。如下图所示，从普通的proposal分布采样替换为用极大似然估计提升采样的质量。这也就是gmapping算法中的做法。
方法二：gmapping算法通过最近一帧的观测（scan），把proposal分布限制在一个狭小的有效区域。然后在正常的对proposal分布进行采样。该方法是gmapping论文中详细描述的方法。
如果proposal分布用激光匹配来表示，则可以把采样范围限制在一个比较小的区域，因此可以用更少的粒子即覆盖了机器人位姿的概率分布。也就是说将proposal分布从里程计的观测模型变换到了激光雷达观测模型。具体公式推导如下所示：
从上图可以看到激光雷达观测模型方差较小，假设其服从高斯分布，使用多元正态分布公式即可进行计算，于是求解高斯分布是下面需要做的。
同方法一，首先通过极大似然估计找到局部极值x_t，认为该局部极值距离高斯分布的均值比较近，于是在位姿x_t附近取了k个位姿；对k个位姿进行打分（位姿匹配），这样我们就具有k个位姿，以及它们与地图匹配的得分；我们又假设它们服从高斯分布，计算出它们的均值和方差之后，即可使用多元正态分布，对机器人位姿的估计进行概率计算。矢量均值和协方差的计算。具体如下式
多元正态分布公式如下，可以根据均值和协方差，直接计算得出后验概率
缓解粒子耗散
问题由来：gmapping算法采用粒子滤波算法对移动机器人轨迹进行估计，必然少不了粒子重采样的过程。随着采样次数的增多，会出现所有粒子都从一个粒子复制而来，这样粒子的多样性就完全丧失了。
目的：缓解粒子耗散
分析：gmapping算法从算法原理上使用粒子滤波，不可避免的要进行粒子重采样，所以，问题实实在在的存在，不能解决，只能从减少重采样的思路走。
方法：gmapping提出选择性重采样的方法，根据所有粒子自身权重的离散程度（也就是权重方差）来决定是否进行粒子重采样的操作。于是就出现了下式，当Neff小于某个阈值，说明粒子差异性过大，进行重采样，否则，不进行。
机器人中常用的坐标系
在机器人系统中，有如下几个比较常见的坐标系，它们分别为机器人坐标系XR YR OR （描述机器人自身信息的坐标系）、传感器坐标系XS YS OS （描述传感器信息的坐标系）、世界坐标系XW YW OW （描述机器人全局信息的坐标系）。世界坐标系是固定不变的，机器人坐标系和传感器坐标系是在世界坐标系下描述的。机器人坐标系和传感器坐标系原点重合但是存在一定的角度，不同的机器人坐标系关系是不同的。当使用传感器数据时，这些坐标系间的关系就是变换矩阵的参数，因为传感器的数据必定是要变换到机器人坐标系或者世界坐标系中使用的（下面将会提到的tf树）。
移动机器人位姿模型
移动机器人的位姿模型就是机器人在世界坐标系下的状态。常用随机变量Xt =(xt ,yt ,θt )来描述t时刻的机器人在世界坐标系下的状态，简称位姿。其中(xt ,yt )表示的在t时刻机器人在世界坐标系下的位置，θt 表示机器人的方向。默认世界坐标系的X正半轴为正方向，逆时针旋转为旋转正方向，初始时刻机器人坐标系和世界坐标系重合。某时刻t机器人的位姿描述如下图所示：
移动机器人里程计模型
简单的说，移动机器人的里程计就是机器人每时每刻在世界坐标系下位姿状态。常用的激光SLAM和导航算法通常都需要移动机器人的里程计作为控制输入。
不同底盘的里程计模型有所不同，而目前大部分的，都是两轮差分轮式机器人。
在介绍里程计模型之前，要先介绍差分轮式机器人的运动学模型，这样可以先了解一下机器人的物理特性。首先明确差分模型的机器人始终做的是以R为半径的圆弧运动。如下图所示，机器人的线速度V、角速度ω，左右轮速用VL和VR表示，用D表示轮间距，D=2d，右轮到旋转中心的距离为L。
ROS端给机器人底盘（STM32端）发送的是机器人要达到的线速度V和角速度ω，而我们底盘控制板需要的是左右轮速VL和VR来进行速度控制。
从上式可以发现，机器人的轮间距影响着我们向左右轮分发速度以及合成角速度，所以这是个我们需要注意的参数，不同的机器人结构，该参数也是不同的，一定不能照搬照用。有了上面坐标系系统模型、位姿模型的基础，里程计就非常简单了。里程计的计算是指以机器人上电时刻为世界坐标系的起点（机器人的航向角是世界坐标系X正方向）开始累积计算任意时刻机器人在世界坐标系下的位姿。通常计算里程计方法是速度积分推算：通过左右电机的编码器测得机器人的左右轮的速度VL和VR，在一个短的时刻△t内，认为机器人是匀速运动，并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理，关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计。具体计算如下图所示：
2D激光雷观测模型
激光雷达通常由精准控制的旋转电机、红外激光发射器、红外接收视觉系统和主控组成。
激光雷达的测距原理分为两种，一种是基于三角测距，另一个是基于TOF（飞行时间）。基于三角测距的激光雷达表现出的特点，价格便宜、中近距离测距较准确、远距离精度差。基于TOF的激光雷达表现出来的特点，价格昂贵、测距精度高、测距范围广、扫描频率高。
在turtlebot上的是较便宜的基于三角测距的低成本激光雷达HLS-LFCD LDS。该激光雷达在每次测距过程中，发射器发射红外激光信号，视觉采集系统接收激光反射信号。在经过主控实时解算后，将激光雷达几何中心到被测物体的距离值以及当时的角度值，通过主控的通信接口发出。HLS-LFCD LDS 工作原理如图所示。
激光雷达在整个SLAM和导航中起着不可替代的作用。第一，通过激光雷达观测数据与地图进行匹配，估计出机器人的位姿；第二，当机器人估计出较准确的位姿时，通过激光雷达的观测数据建立环境地图；第三，在机器人导航过程中，检测地图中的未知障碍物。
源码解读
安装Gmapping
sudo apt-get install ros-kinetic-gmapping #根据您ROS的版本，自行更改即可
Gmapping的话题与服务
之前博客《ROS学习笔记之——gmapping与amcl》也介绍过一部分的gmapping。这里再次看看它订阅与发布的话题
从上图可以看出，gmapping需要订阅机器人的tf话题/tf和激光雷达扫描数据话题/scan，将会发布栅格地图话题/map。而其中，tf是非常重要的，gmapping中的TF变换如下：
什么是TF
之前博客中也介绍过很多次tf了，这里再来重温一下。
在ROS中，很多功能包都需要机器人发布tf树。抽象来讲，一颗tf变换树定义了不同坐标系之间的平移与旋转变换的关系。
具体来说，我们假设有一个机器人，包括一个机器人移动平台和一个安装在平台之上的激光雷达，以这个机器人为例，定义两个坐标系，一个坐标系以机器人移动平台的中心为原点，称为base_link参考系，另一个坐标系以激光雷达的中心为原点，称为base_laser参考系。如下所示
假设在机器人运行过程中，激光雷达可以采集到距离前方障碍物的数据，这些数据当然是以激光雷达为原点的测量值，换句话说，也就是base_laser参考系下的测量值。现在，如果我们想使用这些数据帮助机器人完成避障功能，当然，由于激光雷达在机器人之上，直接使用这些数据不会产生太大的问题，但是激光雷达并不在机器人的中心之上，在极度要求较高的系统中，会始终存在一个雷达与机器人中心的偏差值。这个时候，如果我们采用一种坐标变换，将及激光数据从base_laser参考系变换到base_link参考下，问题不就解决了么。于是我们就需要定义这两个坐标系之间的变换关系。
为了定义这个变换关系，假设我们已知激光雷达安装的位置在机器人的中心点上方20cm，前方10cm处。这就根据这些数据，就足以定义这两个参考系之间的变换关系：当我们获取激光数据后，采用(x: 0.1m, y: 0.0m, z: 0.2m)的坐标变换，就可以将数据从base_laser参考系变换到base_link参考系了。当然，如果要方向变化，采用(x: -0.1m, y: 0.0m, z: -0.20m)的变换即可。
从上边的示例看来，参考系之间的坐标变换好像并不复杂，但是在复杂的系统中，存在的参考系可能远远大于两个，如果我们都使用这种手动的方式进行变换，估计很快你就会被繁杂的坐标关系搞蒙了。ROS提供的tf变换就是为解决这个问题而生的，tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能，我们只需要告诉tf树这些参考系之间的变换公式即可，这颗tf树就可以用树的数据结构管理我们所需要的参考系变换
如上图所示，每两个相邻的坐标系之间都有变换关系，也就是tf变换。我们需要通过机器人的urdf模型文件，向tf树发布机器人相关tf和关节状态。
代码注释
gmapping其实包含了两个包，一个是ros的slam_gmapping，一个是openslam_gmapping。实际上用的时候，单纯ros的slam_gmapping就够了，但是如果需要读懂整个gmapping以及深入了解核心代码，则这些代码都是在openslam中
ros-gmapping的源码在：https://github.com/ros-perception/slam_gmapping
openslam的gmapping：https://github.com/OpenSLAM-org/openslam_gmapping
本人解读源码的时候，习惯把理解都标注于代码中，故此直接把注解及代码放这里哈～
/*
* slam_gmapping
* Copyright (c) 2008, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
/* Author: Brian Gerkey */
/* Modified by: Charles DuHadway */
/**
@mainpage slam_gmapping
@htmlinclude manifest.html
@b slam_gmapping is a wrapper around the GMapping SLAM library. It reads laser
scans and odometry and computes a map. This map can be
written to a file using e.g.
"rosrun map_server map_saver static_map:=dynamic_map"
<hr>
@section topic ROS topics
Subscribes to (name/type):
- @b "scan"/<a href="../../sensor_msgs/html/classstd__msgs_1_1LaserScan.html">sensor_msgs/LaserScan</a> : data from a laser range scanner
- @b "/tf": odometry from the robot
Publishes to (name/type):
- @b "/tf"/tf/tfMessage: position relative to the map
@section services
- @b "~dynamic_map" : returns the map
@section parameters ROS parameters
Reads the following parameters from the parameter server
Parameters used by our GMapping wrapper:
- @b "~throttle_scans": @b [int] throw away every nth laser scan
- @b "~base_frame": @b [string] the tf frame_id to use for the robot base pose
- @b "~map_frame": @b [string] the tf frame_id where the robot pose on the map is published
- @b "~odom_frame": @b [string] the tf frame_id from which odometry is read
- @b "~map_update_interval": @b [double] time in seconds between two recalculations of the map
Parameters used by GMapping itself:
Laser Parameters:
- @b "~/maxRange" @b [double] maximum range of the laser scans. Rays beyond this range get discarded completely. (default: maximum laser range minus 1 cm, as received in the the first LaserScan message)
- @b "~/maxUrange" @b [double] maximum range of the laser scanner that is used for map building (default: same as maxRange)
- @b "~/sigma" @b [double] standard deviation for the scan matching process (cell)
- @b "~/kernelSize" @b [int] search window for the scan matching process
- @b "~/lstep" @b [double] initial search step for scan matching (linear)
- @b "~/astep" @b [double] initial search step for scan matching (angular)
- @b "~/iterations" @b [int] number of refinement steps in the scan matching. The final "precision" for the match is lstep*2^(-iterations) or astep*2^(-iterations), respectively.
- @b "~/lsigma" @b [double] standard deviation for the scan matching process (single laser beam)
- @b "~/ogain" @b [double] gain for smoothing the likelihood
- @b "~/lskip" @b [int] take only every (n+1)th laser ray for computing a match (0 = take all rays)
- @b "~/minimumScore" @b [double] 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)
Motion Model Parameters (all standard deviations of a gaussian noise model)
- @b "~/srr" @b [double] linear noise component (x and y)
- @b "~/stt" @b [double] angular noise component (theta)
- @b "~/srt" @b [double] linear -> angular noise component
- @b "~/str" @b [double] angular -> linear noise component
Others:
- @b "~/linearUpdate" @b [double] the robot only processes new measurements if the robot has moved at least this many meters
- @b "~/angularUpdate" @b [double] the robot only processes new measurements if the robot has turned at least this many rads
- @b "~/resampleThreshold" @b [double] threshold at which the particles get resampled. Higher means more frequent resampling.
- @b "~/particles" @b [int] (fixed) number of particles. Each particle represents a possible trajectory that the robot has traveled
Likelihood sampling (used in scan matching)
- @b "~/llsamplerange" @b [double] linear range
- @b "~/lasamplerange" @b [double] linear step size
- @b "~/llsamplestep" @b [double] linear range
- @b "~/lasamplestep" @b [double] angular step size
Initial map dimensions and resolution:
- @b "~/xmin" @b [double] minimum x position in the map [m]
- @b "~/ymin" @b [double] minimum y position in the map [m]
- @b "~/xmax" @b [double] maximum x position in the map [m]
- @b "~/ymax" @b [double] maximum y position in the map [m]
- @b "~/delta" @b [double] size of one pixel [m]
*/
#include "slam_gmapping.h"
#include <iostream>
#include <time.h>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"
#include "gmapping/sensor/sensor_range/rangesensor.h"
#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
//构造函数
//map_to_odom_一开始是000。
//冒号进行初始化操作
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();//调用初始化函数，在初始化函数中，主要进行参数文件的读入
}
SlamGMapping::SlamGMapping(ros::NodeHandle& nh, ros::NodeHandle& pnh):
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),
laser_count_(0),node_(nh), private_nh_(pnh), scan_filter_sub_(NULL), scan_filter_(NULL), transform_thread_(NULL)
{
seed_ = time(NULL);
init();
}
SlamGMapping::SlamGMapping(long unsigned int seed, long unsigned int max_duration_buffer):
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_(seed), tf_(ros::Duration(max_duration_buffer))
{
init();
}
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();//可以理解为这个一个负责GridSlam整个处理的对象
ROS_ASSERT(gsp_);
tfB_ = new tf::TransformBroadcaster();//tf的广播器
ROS_ASSERT(tfB_);
gsp_laser_ = NULL;
gsp_odom_ = NULL;
got_first_scan_ = false;//将激光雷达的标志位设置为0
got_map_ = false;//一开始将地图的初始化的标志位设置为0
//从文件“/home/kwanwaipang/catkin_ws/src/turtlebot3/turtlebot3_slam/launch/turtlebot3_gmapping.launch”中读入设置的参数
// 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
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_;
}
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);//对应的地图的数据，其实就是.yaml
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);//订阅激光雷达的信息
//tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）
//tf::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);//将激光雷达的信息转到odom frame上
//当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));//调用laserCallback
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));//publishLoop为发布的周期
}
void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
{
double transform_publish_period;
ros::NodeHandle private_nh_("~");
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);
rosbag::Bag bag;
bag.open(bag_fname, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("/tf"));
topics.push_back(scan_topic);
rosbag::View viewall(bag, rosbag::TopicQuery(topics));
// Store up to 5 messages and there error message (if they cannot be processed right away)
std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
foreach(rosbag::MessageInstance const m, viewall)
{
tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
if (cur_tf != NULL) {
for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
{
geometry_msgs::TransformStamped transformStamped;
tf::StampedTransform stampedTf;
transformStamped = cur_tf->transforms[i];
tf::transformStampedMsgToTF(transformStamped, stampedTf);
tf_.setTransform(stampedTf);
}
}
sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
if (s != NULL) {
if (!(ros::Time(s->header.stamp)).is_zero())
{
s_queue.push(std::make_pair(s, ""));
}
// Just like in live processing, only process the latest 5 scans
if (s_queue.size() > 5) {
ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
s_queue.pop();
}
// ignoring un-timestamped tf data
}
// Only process a scan if it has tf data
while (!s_queue.empty())
{
try
{
tf::StampedTransform t;
tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
this->laserCallback(s_queue.front().first);
s_queue.pop();
}
// If tf does not have the data yet
catch(tf2::TransformException& e)
{
// Store the error to display it if we cannot process the data after some time
s_queue.front().second = std::string(e.what());
break;
}
}
}
bag.close();
}
void SlamGMapping::publishLoop(double transform_publish_period){//定义发布的周期
if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);//按这个速率来发布
while(ros::ok()){
publishTransform();
r.sleep();
}
}
//构析函数
SlamGMapping::~SlamGMapping()
{
if(transform_thread_){
transform_thread_->join();
delete transform_thread_;
}
delete gsp_;
if(gsp_laser_)
delete gsp_laser_;
if(gsp_odom_)
delete gsp_odom_;
if (scan_filter_)
delete scan_filter_;
if (scan_filter_sub_)
delete scan_filter_sub_;
}
bool
SlamGMapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t)
{//把激光的坐标系转到odom
// 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;//odom的tf
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());//odom的航向角（累计误差如何纠正）
//gmapping当前获得的位姿由odom提供
gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);
return true;
}
bool
SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
{//初始化地图器
laser_frame_ = scan.header.frame_id;
// Get the laser's pose, relative to base.
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);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return false;
}
// 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));
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_));
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;
}
bool
SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{//这个函数需要scan（激光数据）和odom_pose（里程计位姿）两个参数
//实现了将odom的pose加到gmapping的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];
}
}
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");
//addscan()函数只是将获取到的scan消息作一下处理，过滤掉无效值，将处理过的数据传入processScan()函数,
return gsp_->processScan(reading);//关键
//该函数即是openslam_gmappinig核心算法
//https://www.cnblogs.com/heimazaifei/p/12531150.html
//该函数的功能：
// 1.利用运动模型更新里程计分布。
// 2.利用最近的一次观测来提高proposal分布。(scan-match).
// 3.利用proposal分布+激光雷达数据来确定各个粒子的权重.
// 4.对粒子进行重采样。
}
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
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;//odom下的位姿
if(addScan(*scan, odom_pose))//调用addScan（）函数，此处的odom_pose其实就是gmapping的pose
{// addscan：对新激光，结合里程计数据进行粒子滤波矫正得出最新位置
/// 以下通过getBestParticle的位置（即，当前激光位置）和里程计最终，算出map-to-odom，发布出去
//做了粒子滤波的操作。用RB 粒子滤波，获得最有的位置
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;//mpos为new best 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应该是slam建立的map，让其跟odom的位姿重合。从而使得建立的map围绕着机器人的odom。
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();//考虑了odom的累计误差，由laser scan处理了
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");
}
double
SlamGMapping::computePoseEntropy()
{
double weight_total=0.0;
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
it != gsp_->getParticles().end();
++it)
{
weight_total += it->weight;
}
double entropy = 0.0;
for(std::vector<GMapping::GridSlamProcessor::Particle>::const_iterator it = gsp_->getParticles().begin();
it != gsp_->getParticles().end();
++it)
{
if(it->weight/weight_total > 0.0)
entropy += it->weight/weight_total * log(it->weight/weight_total);
}
return -entropy;
}
// 通过粒子滤波得到的best_particle （一条最优路径。）建图。
void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{//更新地图
ROS_DEBUG("Update map");
boost::mutex::scoped_lock map_lock (map_mutex_);
GMapping::ScanMatcher matcher;// 定义matcher 并设置参数，为地图更新做准备
matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),
gsp_laser_->getPose());//设置laser的参数，里面包含了pose
matcher.setlaserMaxRange(maxRange_);
matcher.setusableRange(maxUrange_);
matcher.setgenerateMap(true);
GMapping::GridSlamProcessor::Particle best =
gsp_->getParticles()[gsp_->getBestParticleIndex()];//获取最好的粒子（跟odom相关联）
std_msgs::Float64 entropy;
entropy.data = computePoseEntropy();
if(entropy.data > 0.0)
entropy_publisher_.publish(entropy);//发布出去（熵）
// 如果没有地图，便初始化一个地图
if(!got_map_) {//如果一开始没有获得地图，则先进行一次初始化？
map_.map.info.resolution = delta_;
map_.map.info.origin.position.x = 0.0;
map_.map.info.origin.position.y = 0.0;
map_.map.info.origin.position.z = 0.0;
map_.map.info.origin.orientation.x = 0.0;
map_.map.info.origin.orientation.y = 0.0;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
GMapping::Point center;
center.x=(xmin_ + xmax_) / 2.0;// xmin，max ，ymin max 地图边界坐标 center 中心坐标
center.y=(ymin_ + ymax_) / 2.0;
//在GMapping中每个粒子都维护了一个地图，并最终采用了最优粒子的地图。它们使用类GMapping::ScanMatcherMap来描述地图
GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_,
delta_);// detla 分辨率
ROS_DEBUG("Trajectory tree:");
//以下得到机器人最优的携带地图路径,重新计算栅格单元的概率.
for(GMapping::GridSlamProcessor::TNode* n = best.node;//最好粒子的pose其实也是相当于前面的mpose（也是跟odom相关联）
n;
n = n->parent)
{
ROS_DEBUG(" %.3f %.3f %.3f",
n->pose.x,
n->pose.y,
n->pose.theta);
if(!n->reading)
{
ROS_DEBUG("Reading is NULL");
continue;
}
//以下是updateMap主要函数
matcher.invalidateActiveArea();//有效区域还没计算，等待计算
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0])); //计算有效区域（跟odom联系了）
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));//匹配扫描，根据Bresenham算法确定激光扫过的单元格，进行改良跟新
}
// the map may have expanded, so resize ros message as well
if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {
// NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
// so we must obtain the bounding box in a different way
GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
xmin_ = wmin.x; ymin_ = wmin.y;
xmax_ = wmax.x; ymax_ = wmax.y;
ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
xmin_, ymin_, xmax_, ymax_);
map_.map.info.width = smap.getMapSizeX();
map_.map.info.height = smap.getMapSizeY();
map_.map.info.origin.position.x = xmin_;
map_.map.info.origin.position.y = ymin_;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
}
// 跟新地图栅格值（看有没有障碍物）
for(int x=0; x < smap.getMapSizeX(); x++)//基于获得的odom的smap
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > occ_thresh_)
{
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
}
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;//地图初始化后，就不需要改变了
//make sure to set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = tf_.resolve( map_frame_ );
sst_.publish(map_.map);//把地图发布出去
sstm_.publish(map_.map.info);//把地图的信息也发布出去
}
bool
SlamGMapping::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)
{
res = map_;
return true;
}
else
return false;
}
void SlamGMapping::publishTransform()
{
map_to_odom_mutex_.lock();
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();
}
消息过滤器（MessageFilter）
http://wiki.ros.org/message_filters
消息过滤器message_filters类似一个消息缓存（起到消息同步的作用），当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。举个例子，比如时间同步器，它接收来自多个源的不同类型的消息，并且仅当它们在具有相同时间戳的每个源上接收到消息时才输出它们，也就是起到了一个消息同步输出的效果。
tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）
对gmapping的理解的补充
参考资料
https://blog.csdn.net/tiancailx/article/details/103804070(消息过滤器)
https://zhuanlan.zhihu.com/p/85946393
https://gaoyichao.com/Xiaotu/?book=turtlebot&title=index
https://blog.csdn.net/roadseek_zw/article/details/53316177
https://blog.csdn.net/qq_29230261/article/details/85116805
https://blog.csdn.net/zhao_ke_xue/article/details/108396430（用urdf给自己的ROS小车编写模型）
https://blog.csdn.net/zhao_ke_xue/article/details/108425922（ROS机器人里程计模型）
https://blog.csdn.net/zhao_ke_xue/article/details/109040676（占据栅格地图构建（Occupancy Grid Map））
https://blog.csdn.net/zhao_ke_xue/article/details/107437438（ROS机器人开机自启动设置）
https://blog.csdn.net/zhao_ke_xue/article/details/105734833（详解2D激光雷达运动畸变去除）
https://blog.csdn.net/zhao_ke_xue/article/details/108138889（ROS小车STM32底层控制代码）
2969
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
