GMapping的ROS封装——初始化
通过分析例程的launch文件， 我们知道例程gmapping_dema.launch运行的是gmapping包中的节点slam_gmapping。 这个launch文件位于包turtlebot_gazebo中， 加载了在turtlebot_navigation包中的gmapping.launch.xml文件， 在该文件中最终加载运行了slam_gmapping的节点。
实际上slam_gmapping只是对GMapping的ROS封装。OpenSLAM给出了GMapping的源码， 只是这份代码相当古老，最近的一次提交还是2012年的时候，源码编译起来都比较费劲。好在人们对它进行了修改， 在ROS的环境下以openslam_gmapping包的形式进行维护。
本文中，详细解读GMapping的ROS封装。后面一些章节中再来拆解openslam_gmapping。
1. 运行过程分析
在分析源码之前，让我们来分析一下这个Demo都做了些什么工作。首先我们打开四个终端，分别在各个终端中运行如下的四条指令，它们依次打开Gazebo仿真环境，用于控制机器人的keyboard_teleop，GMapping demo， 以及查看构建地图用的rviz。详细请参考Turtlebot你好!一文。
$ roslaunch turtlebot_gazebo turtlebot_world.launch
$ roslaunch turtlebot_teleop keyboard_teleop.launch
$ roslaunch turtlebot_gazebo gmapping_demo.launch
$ roslaunch turtlebot_rviz_launchers view_navigation.launch
我们可以通过工具rqt_graph来查看当前ROS系统的运行时图，如下图所示，其中"slam_gmapping"就是我们所关注的GMapping demo。在下图的最左边， 我们可以看到节点turtlebot_teleop_keyboard发布了主题/cmd_vel_mux/input/teleop。这就是下发的机器人控制指令，它被节点mobile_base_nodelet_manager订阅。
mobile_base_nodelet_manager是turtlebot_world.launch文件开启的ROS节点，它是机器人的控制器， 运行的是yocs_cmd_vel_mux中的CmdVelMuxNodelet。这个节点可以接收来自多个节点的控制指令， 它根据一些预设的优先级来决定机器人的运动，其输出就是主题"/mobile_base/commands/velocity"。
主题"/mobile_base/commands/velocity"被Gazebo的底盘控制插件所订阅，该插件将根据这一主题的消息计算机器人的里程计信息， 并将之以tf的形式发布。于此同时Gazebo插件还负责计算机器人身上各个关节的运动情况，并以"joint_states"的形式发布。节点robot_state_publisher订阅这一主题，并维护机器人上各个关节之间的变换关系， 以tf的形式发布。
此外，Gazebo的插件还发布机器人上的双目摄像头数据，它们被laserscan_nodelte_manager订阅并将其深度信息转换为激光扫描数据，以主题"/scan"的形式发布。 本文的主角slam_gmapping收集激光扫描数据，结合机器人里程计，实时计算地图信息，并以主题"/map"的形式发布。
$ rosnode info /slam_gmapping
----------------------------------------------------
Node [/slam_gmapping]
Publications:
* /map_metadata [nav_msgs/MapMetaData]
* /tf [tf2_msgs/TFMessage]
* /map [nav_msgs/OccupancyGrid]
* /rosout [rosgraph_msgs/Log]
* /slam_gmapping/entropy [std_msgs/Float64]
Subscriptions:
* /tf [tf2_msgs/TFMessage]
* /scan [sensor_msgs/LaserScan]
* /tf_static [tf2_msgs/TFMessage]
* /clock [rosgraph_msgs/Clock]
Services:
* /slam_gmapping/set_logger_level
* /slam_gmapping/get_loggers
* /dynamic_map
如右边的代码所示，在终端中运行rosnode info /slam_gmapping就可以查看其详细的运行信息。从指令的运行结果来看，slam_gmapping发布了5个主题。
其中，[/map_metadata]是关于地图的一些配置数据，其消息类型为[nav_msgs/MapMetaData]，它描述了地图的分辨率、尺寸、原点等信息。实际上这一信息是[/map]中描述的占用栅格地图数据的一部分。
[/map]使用消息[nav_msgs/OccupancyGrid]来描述占用栅格地图，它由如下的三个字段构成。
- std_msgs/Header header。消息头，其中记录有时间戳和帧ID编号。
- nav_msgs/MapMetaData info。地图的配置数据，包括分辨率、尺寸、原点信息等，就是[/map_metadata]发布的消息内容。
- int8[] data。以row-major的形式记录了占用栅格地图数据，它用[0-100]的数值来表示对应栅格的占用概率，如果未知则标记为-1。
[/slam_gmapping/entropy]描述了粒子滤波器中关于机器人位姿的不确定性。[/tf]是ROS系统的tf坐标变换库订阅和发布的主题， tf库坐标变换进行了封装，很多时候我们不太关心主题[/tf]中的消息内容，而是通过tf库中的接口来直接获取坐标变换关系。slam_gmapping发布的这个tf主要是描述地图到里程计的坐标变换。
此外，[/slam_gmapping]还订阅了四个主题。[/tf]来自于机器人里程计的坐标变换，[/scan]则是激光雷达的扫描信息。[/tf_static]是tf2中添加的新特性，目的是提供一个不变的在任何时刻都成立的变换矩阵。 [/clock]则是来自Gazebo的主题，与仿真时间有关。
我们看到，[/slam_gmapping]还提供了三个服务，其中set_logger_level和get_loggers与日志相关，而dynamic_map则返回当前构建的地图。
2. 源码结构
$ git clone https://github.com/ros-perception/slam_gmapping.git
$ cd slam_gmapping
$ tree .
.
├── gmapping
│ ├── CHANGELOG.rst
│ ├── CMakeLists.txt
│ ├── launch
│ │ └── slam_gmapping_pr2.launch
│ ├── nodelet_plugins.xml
│ ├── package.xml
│ ├── src
│ │ ├── main.cpp
│ │ ├── nodelet.cpp
│ │ ├── replay.cpp
│ │ ├── slam_gmapping.cpp
│ │ └── slam_gmapping.h
│ └── test
│ ├── basic_localization_laser_different_beamcount.test
│ ├── basic_localization_stage.launch
│ ├── basic_localization_stage_replay2.launch
│ ├── basic_localization_stage_replay.launch
│ ├── basic_localization_symmetry.launch
│ ├── basic_localization_upside_down.launch
│ ├── rtest.cpp
│ └── test_map.py
└── slam_gmapping
├── CHANGELOG.rst
├── CMakeLists.txt
└── package.xml
5 directories, 21 files
如右侧代码所示，我们通过Git工具把slam_gmapping从github上拉下来，并查看其目录结构。
我们会看到有两个包:gmapping和slam_gmapping。其中slam_gmapping是一个所谓的mata-package，这是ROS系统早期的一个概念，现在已经不再使用了，所以不用管它。 gmapping才是对GMapping算法的ROS封装。
查看编译指导文件CMakeLists.txt，一共添加了两个可执行文件slam_mapping和slam_mapping_replay，以及一个链接库文件slam_nodelet。例程中运行的是slam_mapping， 它涉及到slam_gmapping.cpp, main.cpp, 和slam_gmapping.h三个源文件。
add_executable(slam_gmapping src/slam_gmapping.cpp src/main.cpp)
add_library(slam_gmapping_nodelet src/slam_gmapping.cpp
src/nodelet.cpp)
add_executable(slam_gmapping_replay src/slam_gmapping.cpp
src/replay.cpp)
它的main.cpp很简短，在main函数的一开始先初始化ROS的工作环境，接着构建了一个SlamGMapping的对象gn，并通过成员函数startLiveSlam开始建图，最后调用ros::spin()开启消息循环。
#include <ros/ros.h>
#include <slam_gmapping.h>
int main(int argc, char** argv) {
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();
return(0);
}
3. SlamGMapping的初始化过程和参数
SlamGMapping对象gn用ROS的接口完成了对GMapping的封装。在它的默认构造函数中，初始化了地图到里程计的坐标变换map_to_odom_，雷达计数器laser_count_。 使用私有的命名空间构建了ROS的节点句柄private_nh_，用NULL为一些指针赋予初值。在函数体中，调用系统函数time获取当前的系统时间，这是一种常用的初始化随机数种子的方法。 然后就调用成员函数init()完成初始化过程。
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();
}
表 1. GMapping参数配置
| GMapping封装 | ||
|---|---|---|
| ~throttle_scans | int | 每当接收到n个激光扫描数据，就进行一次更新。 |
| ~base_frame | string | tf框架下的机器人坐标系名称。 |
| ~map_frame | string | tf框架下的地图坐标系名称。 |
| ~odom_frame | string | tf框架下里程计坐标系名称。 |
| ~map_update_interval | double | 地图的更新周期，既连续两次地图计算的时间间隔，时间单位为s。 |
| 关于激光传感器的配置参数 | ||
| ~/maxRange | double | 传感器的最大测量距离，如果采集到比该值更大的数值将抛弃之。在缺省的情况下， 以第一次扫描中的最大值减去1cm作为maxRange的值。 |
| ~/maxUrange | double | 用于建图的传感器最大值，默认情况下与maxRange相同。 |
| ~/sigma | double | 扫描匹配过程中cell的标准差。 |
| ~/kernelSize | int | 扫描匹配过程的搜索窗口大小。 |
| ~/lstep | double | 扫描匹配的初始距离步长。 |
| ~/astep | double | 扫描匹配的初始角度步长。 |
| ~/iterations | int | 扫描匹配的迭代步长的细分数量，最终距离和角度的匹配精度分别为(lstep * 2 ^ (-iterations))和(astep * 2 ^ (-iterations)) |
| ~/lsigma | double | 扫描匹配过程中单个激光扫描束的标准差。 |
| ~/ogain | double | 似然度平滑的增益。 |
| ~/lskip | int | 每个n+1个扫描进行一次扫描匹配，取值为0时表示每次扫描之后都进行一次匹配。 |
| ~/minimumScore | double | 判定扫描匹配质量足够好的最小值。默认值为0， 最大值可以达到600+，如果遇到了"跳跃"现象可以尝试提高该值。 |
| 运动模型中高斯噪声的标准差 | ||
| ~/srr | double | 位置的噪声项。 |
| ~/stt | double | 方位角的噪声项。 |
| ~/srt | double | 位置到方位角的协方差项。 |
| ~/str | double | 方位角到位置的协方差项。 |
| 扫描匹配所用的似然度采样特性 | ||
| ~/llsamplerange | double | 距离采样范围。 |
| ~/llsamplestep | double | 距离采样步长。 |
| ~/lasamplerange | double | 角度采样范围。 |
| ~/lasamplestep | double | 角度采样步长。 |
| ~/occ_thresh | double | 占用概率阈值。 |
| 初始地图的尺度和分辨率 | ||
| ~/xmin | double | 地图的最小x值。 |
| ~/xmax | double | 地图的最大x值。 |
| ~/ymin | double | 地图的最小y值。 |
| ~/ymax | double | 地图的最大y值。 |
| ~/delta | double | 地图中一个像素的尺度。 |
| 粒子滤波器的配置 | ||
| ~/linearUpdate | double | 只有当机器人至少运动了linearUpdate的距离之后才进行一次新的测量。 |
| ~/angularUpdate | double | 只有当机器人至少转动了angularUpdate的角度之后才进行一次新的测量。 |
| ~/resampleThreshold | double | 粒子重采样的阈值。只有当评价粒子相似度的指标\(N_{eff}\)小于该阈值时才进行重采样，所以降低该值意味着提高重采样的频率。 |
| ~/particles | int | 粒子的数量，每个粒子都表示一条机器人的可能路径。 |
在init()函数中，首先构建了一个GMapping::GridSlamProcessor对象gsp_。 这个类就是基本的GridFastSLAM算法， 它用Rao-Blackwellized粒子滤波器完成SLAM工作，每个粒子都有一个关于地图和机器人位姿的估计。
void SlamGMapping::init() {
gsp_ = new GMapping::GridSlamProcessor();
ROS_ASSERT(gsp_);
接着创建了一个tf广播器tfB_，用于发布从地图到里程计的坐标变换map_to_odom_。
tfB_ = new tf::TransformBroadcaster();
ROS_ASSERT(tfB_);
下面暂时将栅格SLAM算法中用到的激光传感器和里程计对象置为空，它们将在获得第一次扫描数据时构建对象。变量got_first_scan_标识着是否接收到了第一次的扫描数据， got_map_则表示已经生成了地图。在初始化时这两个变量都应该为false。
gsp_laser_ = NULL;
gsp_odom_ = NULL;
got_first_scan_ = false;
got_map_ = false;
接下来就是各种参数的配置。需要配置的参数有很多，大体上都是以类似下面的形式进行初始化的。首先通过私有句柄private_nh_从参数服务器上请求相关的参数，如果查询发现没有该参数的配置， 就赋予一个缺省值。右边的表1中详细列出了各种参数的数据类型和功能。
// 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";
// 省略剩余的配置，详细参考源码
}
4. initMapper
虽然我们已经构建了GridSlamProcessor对象gsp_，并且从ROS的参数服务器获得了各种配置参数。但是这些参数配置并没有在对象gsp_上生效。 我们还需要调用函数initMapper完成建图引擎的配置。在下文中的地图更新封装介绍中， 我们会看到这一操作将被推迟到第一次获得激光扫描数据之时。
在该函数中，根据输入的传感器测量数据，我们可以获得激光传感器的坐标系名称"scan.header.frame_id"，以及获得传感器测量值时刻的时间戳。根据这两个数据， 我们构建了一个打戳的单元向量ident。将该参数和ident一起提供给tf的库函数transformPose，就可以从其输出参数laser_pose中获得激光传感器相对于机器人基座的坐标变换。
需要注意的是，在tf的运行机制中。由于tf会把监听到的内容放到一个缓存中。我们通过transformPose获取变换关系，是通过查询这个缓存来实现的。 所以，我们获取的数据不能保证实时性，会有一定的延迟。也有可能无法获得，因此这个函数在运行过程中会抛出异常，所以这里使用try-catch语句捕获这个异常并返回。
bool SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan) {
laser_frame_ = scan.header.frame_id;
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) {
return false;
}
接下来在机器人坐标系下，在雷达传感器的正上方1m处构建一个点，然后将它转换到激光雷达的坐标系下。
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);
} catch(tf::TransformException& e) {
return false;
}
构建的这个点将被用来判定传感器的安装方式。GMapping要求激光雷达一定是水平安装的，也就是说雷达的Z轴要么朝上，要么朝下。如果传感器是水平安装的， 那么从机器人坐标系下转换到传感器坐标系下，该点的z轴数据应当接近为1或者-1。
if (fabs(fabs(up.z()) - 1) > 0.001)
return false;
从传感器消息中获取激光扫描数据量和中间角度。
gsp_laser_beam_count_ = scan.ranges.size();
double angle_center = (scan.angle_min + scan.angle_max)/2;
接下来我们根据刚刚判定的传感器安装方式以及中间角度，来初始化传感器的中心位置centered_laser_pose_。另外扫描的最小角度和最大角度了说明扫描方向， 如果扫描反向，那么在以后的数据处理过程中，将以相反的顺序处理扫描数据。这里用变量do_reverse_range_来记录这一特性。
if (up.z() > 0) {
do_reverse_range_ = scan.angle_min > scan.angle_max;
centered_laser_pose_ = tf::Stamped(tf::Transform(tf::createQuaternionFromRPY(0,0,angle_center),
tf::Vector3(0,0,0)), ros::Time::now(), laser_frame_);
} else {
// 传感器倒着安装，代码略
}
然后，我们根据扫描的数据长度和增长步长初始化雷达的扫描角度表laser_angles_，该表中的角度都是从小到大增长的。
laser_angles_.resize(scan.ranges.size());
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);
}
接下来配置参数maxRange和maxUrange。默认情况下，maxRange的取值为第一次扫描最大值减去1cm，maxUrange默认与maxRange取值相同。
if(!private_nh_.getParam("maxRange", maxRange_))
maxRange_ = scan.range_max - 0.01;
if(!private_nh_.getParam("maxUrange", maxUrange_))
maxUrange_ = maxRange_;
接着构建距离传感器对象gsp_laser_。该对象的名称一定是"FLASER"。此外传递的扫描角度增量是绝对值，如果实际的传感器扫描方向与之相反， 我们的封装会在传递给建图引擎的时候就将之反转过来。
GMapping::OrientedPoint gmap_pose(0, 0, 0);
gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_,
fabs(scan.angle_increment), gmap_pose, 0.0, maxRange_);
然后将新建的距离传感器对象添加到建图引擎中。这里的类型GMapping::SensorMap实际上就是标准库中的map，在openslam_gmapping的头文件
sensor.h中定义为
typedef std::map<std::string, Sensor*> SensorMap;，在字符串与传感器对象之间建立映射关系。
GMapping::SensorMap smap;
smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_));
gsp_->setSensorMap(smap);
接着创建里程计对象，获取初始位置。并根据这一初始位置完成建图引擎的初始化操作。
gsp_odom_ = new GMapping::OdometrySensor(odom_frame_);
GMapping::OrientedPoint initialPose;
if(!getOdomPose(initialPose, scan.header.stamp))
initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0);
// 省略了对建图引擎的参数配置语句
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_, delta_, initialPose);
}
5. 总结
在本文中，我们解读了slam_gmapping对GMapping建图引擎的封装的初始化过程。GMapping使用激光传感器和里程计数据进行SLAM建图，它是一种Rao-Blackwellized粒子滤波器。 我们还详细介绍了GMapping相关的各种参数配置。