ROS中slam_gmappingmap_server源码解读及其librviz的使用
Posted lwlv
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了ROS中slam_gmappingmap_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!
以上是关于ROS中slam_gmappingmap_server源码解读及其librviz的使用的主要内容,如果未能解决你的问题,请参考以下文章