ROS中slam_gmapping、map_server源码解读及其librviz的使用

 SLAM全称simultaneous localization and mapping，即实时定位与地图构建。也就是说导航离不开地图，目前常用的地图构建方法有三种：

1、gmapping，一种基于粒子滤波的算法，依赖里程计数据。ROS中的slam_gmapping包也是调用了openslam_gmapping开源算法。openslam_gmapping的介绍及其源码可在http://wiki.ros.org/openslam_gmapping?distro=indigo和https://github.com/ros-perception/openslam_gmapping上看到。

2、hector slam，不需要用到里程计，对于雷达帧率要求很高（40Hz）。关于hector slam的介绍可查阅网站http://wiki.ros.org/hector_slam和https://github.com/tu-darmstadt-ros-pkg/hector_slam（源码）。

3、cartographer，累计误差较小，由谷歌推出，效果还是挺炫的。其介绍和源码的相关网站为http://wiki.ros.org/cartographer和https://github.com/googlecartographer/cartographer。

关于这三种方法，具体可查看博文《Gmapping、hector、Cartographer三种激光SLAM算法简单对比》。

 

下面言归正传说说ros中slam_gmapping包和map_server包的作用。

一、slam_gmapping和map_server

这两个包是在建图时是用得最多的。

1、slam_gmapping

前面也所过，slam_gmapping包也是依赖开源openslam_gmapping库。下载openslam_gmapping后编译会生成几个动态库，libgridfastslam.so，libscanmatcher.so，libsensor_base.so，libsensor_odometry.so，libsensor_range.so和libutils.so，源码没有仔细研究，不过看名字也大概可以知道它们的作用。

在slma_gmapping包中编译后会生成好几个节点，不过真正需要调用的建图节点的主要实现在slam_gmapping.cpp中，通过roslaunch slam_gmapping slam_gmapping.launch或者rosrun gmapping slam_gmapping scan:=scan启动，开始扫图。

首先是初始化，载入或者配置参数。

然后调用startLiveSlam函数开启建图模式，在这个函数中会发布三个话题，/enropy(机器人姿态分布熵的估计)，/map(建图过程中的数据，用占有网格数据描述，其值在0~255之间)，/map_metadta(地图的描述消息)；会发布一个服务，/dynamic_map，也是为了获取地图数据；会订阅激光数据和tf坐标转换后的数据，还用到了message_filters方法，也是为了加快数据传输效率。此外，还开启了一个线程，用于不断广播map和odom之间的坐标变换。

其次的关键就是对获取的点云scan数据进行处理，这个在回调函数laserCallback中实现。一旦第一次获取到scan数据，会调用initMapper函数形成一个初始的地图形状，这个函数里面也是进行了大量tf变换，然后通过getOdomPose函数获取机器人初始位置，正常情况下该函数只会执行一次，再往后获取到的scan数据则由addScan函数处理了。addScan函数中会先得到里程计数据，然后处理scan数据。最后更新地图数据，由updateMap函数完成，并调用computePoseEntropy函数计算机器人位置分布熵，同时发布map和map_metadata消息。

slam_gmapping包的主要工作就这些，就是将数据处理为了在ros通讯机制下的，其核心算法还是在openslam_gmapping里面实现的。

2、map_server

map_server的功能就是地图服务器。编译map_server后会生成map_server_image_loader动态库，这个用于处理pgm格式的图片，其依赖了SDL库；还会生成map_server和map_saver节点。

map_server节点的源代码很容易看懂，主要就是为了加载pgm格式的地图，用yaml文件描述的，同时发布map_metadata和map话题，以及static_map服务，其目的都是为了方便其他节点获取到地图数据。指令为rosrun map_server map_server mymap.yaml。

map_saver的作用就是为了将slam_gmapping建图数据保存下来，主要就是订阅map消息。rosrun map_server map_saver -f mymap。

具体操作及其参数说明可参见http://wiki.ros.org/map_server。

二、librviz的使用

前面的操作都是在指令下进行的，如果想看到建图时的实时效果可通过rviz软件查看。但是如果想自己开发软件，可使用librviz。正如官网所说，rviz不仅仅是一个可视化工具，还是一个库，可被其他程序所调用。

官网给的例子还是挺简单的。此外，rviz用的qt4库，至少ros indigo版本是的。

加载rviz显示的关键代码如下：

rviz::RenderPanel * render_panel_;
render_panel_ = new rviz::RenderPanel;
rviz::VisualizationManager *manager_;
manager_ = new rviz::VisualizationManager(render_panel_);
render_panel_->initialize(manager_->getSceneManager(), manager_);
manager_->initialize();
manager_->startUpdate();

rviz::Display *map = manager_->createDisplay("rviz/Map", "adjustable map", true);
map->subProp("Topic")->setValue("/map");

首先是声明一个rviz的RenderPanel，这个面板类最终也是继承了QWidget，然后声明一个可视化管理类，继承于QObject，再初始化，VisualizationManager就可以开始不断更新了。上面代码段显示了如何加载地图，也是通过创建一个显示(createDisplay)，然后添加topic值。

结合官方给的例子，可以自写一个调用librviz的小程序，运行效果如下：

 

该小程序可以在https://github.com/WelinLee/ROS_QT_GUI中的vizlib_test包中找到。

 

Enjoy!