此篇博客转自本作者在古月居的博客： https://www.guyuehome.com/34537
前言：
在研一的时候，由于XXX项目的需要，其中一项就是需要测试Gmapping建图，自然也要求使用ROS系统。由于之前经常在webots中进行仿真，所以想先在仿真中完成代码，再迁移实体。还有的是项目需要使用的ORB-SLAM3，便又测试了单目里程估计。这部分代码已经上传到我的Github上供大家参考（研究生小白菜福利）。这里就不贴代码了，文中会说文件和函数。先上一波图，说明是可以的。
gmapping是基于RBpf粒子滤波的，定位和建图分离，是先定位后建图的思想。机器人不断运动运动，不断观测，降低自己位置的不确定度，最后精确定位。如果只是想使用，通过ROS可以很方便调用这部分算法。对于具体的不同环境可以调节参数来获得良好效果。参数可以在gampping的node下面进行修改，每个不同的属性可以在ROS wiki gmapping网站上查询。在我的代码中，节点描述和启动在mob.launch文件中实现。
ORB-SLAM3是开源视觉SLAM库，建的稀疏图，emmmmm我也不是专门研究这个视觉定位建图方向的，了解浅薄，没啥太多实践，所以这里就不在BB了：）。在我的这个项目里用来做里程估计，给的代码是没有给webots中的相机进行标定内参的，就是随便给的，主要的文件就是orb3_odom.cpp，主要功能函数就是GrabImage。启动这部分功能，运行orb3.launch就可以了。另外要很准确的在webots标定相机，没有试过，这个还得靠大家一起探索，如果有好的方法可以留言。webots的相机都是理想的（没有畸变），查查官网，估计也不是难事。再不行也可以通过一些手段反向得到相机内参，也不是不可以。
在我的工程中，主要就是矫正了坐标系，测试了下效果（里程的单位不是准确的），感觉还行。突出的贡献，主要是如何和webots结合在一起仿真使用吧。题外话：webots搭建世界环境是很方便的，做视觉SLAM的可以在这个虚拟环境中先进行测试，然后在部署到硬件上，感觉开发会快不少。
Webots平台可以说是足够简单容易上手的，基于ODE物理引擎。凡事有个但是。但是当大家想把webots和ROS一起用的时候似乎就不是那么流行。很多做ROS物理仿真的人都是在GAZEBO中实现的，但是总是有人想要用webots和ROS。总的来说，webots支持ROS还不错，在ubuntu下面只要装额外的包即可webots_ros。按照官网说的，装好环境后
对于传感器初始化，我写成了一个函数，在mob.hpp下的 __Order_Init_Sensor。通过ServiceClient的形式配置传感器的time_step和使能。然后，通过 rostopic list 你就会发现传感器话题。电机貌似直接给指令，不用使能。使能的传感器会以time_step的间隔发送topic，订阅后，只要在callback函数里面处理数据就可以了。另外，给电机指令的函数我封装成 mob.hpp下 __Order_Float。 工程中的传感器没多少，如果要用其他的传感器，举一反三，照着来就会了。
总的来说特别方便。就是代码的内容相比没有ROS的时候多了点，这个自己封装下就好。
还有就是webots的启动可以通过webots.launch运行脚本webots_launcher.py启动，里面的参数配置仔细看下就好。
上面的内容都弄好了，ROS的topic接口衔接以下就好。对于ROS默认坐标系和Webots不同的问题，我的解决办法是自己在callback函数里面进行矫正。在这个工程里，我没有控制小车pioneer3，而是直接给了个速度，向前进，但足以说明工程已经走通。工程里面，可能会多一些之前没咋见过的东西，这些都是Webots提供的一些.msg(消息格式)啊，或者.srv（ROS Service）文件，不要太在意。
还有就是这个代码需要修改launch文件中的一些路径，不是简单下载编译就好了。
工程在编译好后，先运行mob.launch（gmapping），再运行orb3.launch (orb3)。
代码可以在这里下载。
最后，如果有啥问题，可以留言，古德拉克。
