差速小车的Cartographer建图

在之前的一些文章中，我们创建了DiffCart的仿真模型并能通过键盘控制它。还为之提供了里程计和IMU用于估计机器人的位姿。现在我们来给DiffCart装上激光雷达，并使用Cartographer进行建图。

Cartographer是google的工程师开发的一个用于室内实时建图的SLAM解决方案。目前这种方案支持单线激光的2D建图，也支持多线激光的3D建图。我们的初衷是使用多线激光进行3D建图，在写本文的demo的时候，虽然成功地对接上了Cartographer的3D建图方法，但是不清楚是算力不够还是建图参数没有调整， 3D定位建图过程非常缓慢，需要DiffCart走走停停才能看到建图效果。因此，我们还是额外提供了一个2D建图的demo，它就要流畅很多。

本文中，我们先进行3D建图，再在此基础上进行修改提供2D的建图方案。我们将记录下demo的编写调试过程，以及中间遇到的坑和相关思考。

1. 安装多线激光

安装多线激光，我一开始的想法是直接把之前创建的32线的Velodyne雷达模型直接include到DiffCart的模型文件中。但是出于两方面的考虑，最后还是没有这么做。

其一，我不清楚如何把这个雷达固定到小车上。我知道在SDF文件的<include>标签是可以添加<pose>子标签来指定放置位置的，通过fixed类型的joint可以把两个link固连在一起。但是DiffCart和Velodyne的模型中都有多个link，我不知道该怎样指定具体的link固连起来。这方面的资料比较少，扒源码也比较麻烦。
其二，demo的编写过程并没有过多的考虑模块化的问题。如果直接把模型文件include进来，那么以后我们将维护两个demo的插件。在系统的模块化设计比较合理，耦合度比较低的情况下，这样处理还是比较方便的。但是我根本没考虑过这方面问题，而且还想着以后逐渐剥离ROS系统，纯粹使用Gazebo进行仿真。

所以，我们还是参考Velodyne仿真插件的套路，在DiffCart的模型文件中新增了一个link来安装激光雷达。如下图所示，我们在[−0.10.00.24]T的位置上安放了雷达的link，这个位置在xy平面上的投影正好与小车两轮的中点投影重合。我们还使用一个fixed的joint将之与车体固连起来。如下面右图所示，小车上面那个圆柱就是新安装的雷达。

这仍然是一个32线的激光雷达，每线有90个采样点，更新频率是5Hz。一开始每线的采样点也是2187个，而且更新频率是10Hz。发现仿真跑不动，小车走起来有明显的卡滞，而且Cartographer几乎没有输出。所以就调低了采样频率和采样点数量。然后，我们在仿真插件中按照Velodyne仿真插件的套路，订阅激光传感器的数据消息，并在回调函数中解析消息，填充ROS的sensor_msgs::PointCloud2消息并发布。

2. 3D建图

现在，我们已经准备好了传感器和控制器，可以开始对接Cartographer进行建图了。我们曾经按照官方教程安装过Cartographer，但是在另外一个ROS工作空间下进行的，使用起来多少有些麻烦。可以将两个工作空间合并，但还是有点麻烦，所以我直接按照如下的指令安装了ROS官方维护的Cartographer。

        $ sudo apt install ros-melodic-cartographer*

在launch子目录下新建一个launch脚本diffcart_3d_cartographer.launch，参考cartographer_ros的官方demo，如下面的代码片段所示，我们在其中的第1到7行中运行cartographer_node节点，通过参数args指定配置文件所在目录和配置文件。同时还将cartographer_node中所用的主题"imu"和"points2"重映射为我们在仿真插件中发布的关于IMU数据和点云数据主题。最后第7,8行中运行的节点cartographer_occupancy_grid_node，将Cartographer生成的子图合并成为一个占用删格地图并通过ROS的主题发布出去。

        <node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node" args="-configuration_directory $(find gazebo_demos)/config -configuration_basename diffcart_3d.lua" output="screen"><remap from="imu" to="/imu_data"/><remap from="points2" to="/point_cloud"/></node><node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

为了方便调整Cartographer的运行参数，我们在gazebo_demos的根目录下创建了一个子目录config，并将cartographer_ros中的配置文件backpack_3d.lua文件拷贝过来，命名为diffcart_3d.lua。官方教程中，详细的说明了各个配置项的意义。在下表中，我们列出一些比较重要的字段。

字段	值	说明
map_frame	"map"	地图坐标系名称
tracking_frame	"base_link"	Cartographer将要跟踪的机器人坐标系名称
provide_odom_frame	true	Cartographer将使用Local SLAM的位姿估计作为里程计
odom_frame	"odom"	里程计坐标系名称。
use_odometry	true	在Local SLAM中使用里程计信息，Cartographer将订阅主题"odom"接收nav_msgs/Odometry类型的消息。
num_point_clouds	1	激光点云主题数量，由于我们只有一个激光雷达，所以Cartographer将订阅主题"points2"接收sensor_msgs/PointCloud2类型的消息。
MAP_BUILDER.use_trajectory_builder_3d	true	启用3D建图引擎。

这些配置项要求Cartographer维护从"map"到"odom"再到"base_link"的坐标变换。我们的插件只需要提供"/odom", "/imu_data"和"/point_cloud"三个主题的消息就可以驱动Cartographer进行定位和建图了。所以，我们对DiffCart的里程计做了一些简单的修改，不再发布从"odom"到"base_link"的tf变换消息，而是nav_msgs/Odometry类型的消息。

此外，为了防止Cartographer出现一些莫名奇妙的错误，我们增加了"imu_link"和"laser_link"两个坐标系，并通过tf工具直接发布静态坐标关系，将之与机器人坐标系"base_link"固连在一起。如下面的代码片段所示：

        <node pkg="tf" type="static_transform_publisher" name="laser_base_link_transform" args="0 0 0 0 0 0 base_link laser_link 100"/><node pkg="tf" type="static_transform_publisher" name="imu_base_link_transform" args="0 0 0 0 0 0 base_link imu_link 100" />

我们还需要设置ROS运行参数"/use_sim_time"为true，来统一ROS与Gazebo之间的时间体系。调用脚本"bringup_gazebo.sh"运行Gazebo加载仿真环境。世界文件diffcart_willowgarage.world还从Gazebo官方维护的模型仓库gazebo_models中引入了"willowgarage"模型，提供了一个室内场景。

        <param name="/use_sim_time" value="true" /><node name="gazebo" pkg="gazebo_demos" type="bringup_gazebo.sh"args="$(find gazebo_demos)/worlds/diffcart_willowgarage.world" output="screen"/>

完成必要的准备工作之后，我们就可以通过指令$ roslaunch gazebo_demos diffcart_3d_cartographer.launch运行launch脚本，开始Cartographer建图仿真了。下面左右两幅图分别是Gazebo仿真环境和rviz中激光扫描和建图过程的截图。

由于系统运行十分缓慢，Cartographer响应很迟钝，需要走走停停才能看到建图效果，所以这里的截图只是走了一小段而已。估计需要比较细致的调整一下Cartographer的运行参数才能看到比较流畅的效果。

3. 2D建图

我还是希望能够看到一个比较流畅的效果的，可能3D的建图方法对算力的要求比较高，所以我们将DiffCart上的32线激光改成了单线激光，进行2D建图。主要有以下几个方面的改动：

新建了一个模型diffcart_with_single_laser，安装的是单线激光。
提供了一个世界文件将其中的DiffCart模型替换为单线激光版本的。
在config子目录下增加用于2D建图的配置文件。
针对单线激光修改了接收激光扫描数据的回调函数OnLaserScanMsg，具体修改内容参见源码，不再细述。
增加用于2D建图的launch脚本。

修改完成之后，通过指令$ roslaunch gazebo_demos diffcart_2d_cartographer.launch运行2D建图的demo，并使用键盘控制DiffCart在仿真世界中运动一段时间后，就可以看到下图的效果。整个过程还是比较流畅的，而且可以看到运动的轨迹和不断增加的约束。

4. 完

要运行Cartographer，我们需要根据传感器的类型和建图方法提供cartographer_ros的系统的*.lua配置文件。并根据该配置文件组织传感器数据消息的主题，以及相关的坐标系统。我们的demo中Cartographer维护了地图、里程计和机器人本体之间的坐标关系。修改配置项provide_odom_frame为false，Cartographer将不再提供局部的位姿估计，需要用户自己根据实际情况提供。由于SLAM问题本身就有定位和建图两个方面，所以建议将该配置项置为true。