官方的ROS封装——cartographer_ros

在上一章中，我们安装了cartographer并通过官方提供的数据运行了一个2D建图的demo。我们了解到Cartographer通过最优位姿估计，实时的把激光扫描数据添加到当前维护的局部地图中。并在后台进行进行闭环检测对整个地图和历史轨迹进行优化，得到较为准确的全局地图信息。

通过简单的分析demo运行时的系统节点和发布的主题，发现实际与cartographer相关的ROS节点只有/cartographer_node和/cartographer_occupancy_grid_node两个。其中/cartographer_node是整个程序的入口，它接收激光雷达和IMU的传感数据完成定位和子图的构建。/cartographer_occupancy_grid_node用于构建占用栅格地图。

这两个节点都是官方ROS封装cartographer_ros的一部分，本文我们将分析该包。

1. cartographer_ros的目录结构

通过如下的指令，我们可以切换到cartographer_ros的源码目录下，并列举出其中的目录和文件名称：

该目录下的文件都是一些说明性质的以及不同ROS环境下的Docker配置，不表。目录docs中是些说明文档，jenkins目录中是关于持续集成工具Jenkins的相关配置， scripts中存放的是一些与安装相关的自动化脚本。与Cartographer相关的只有cartographer_ros、cartographer_ros_msgs、cartographer_rviz三个目录。

其中，cartographer_ros_msgs中定义了很多msg和srv文件，它们为Cartographer定义了各种消息类型。cartographer_rviz是与可视化相关的工程。我们只关心这里的cartographer_ros，它才是官方ROS封装的主体，让我们进入该目录查看一下：

其中CHANGELOG.rst没什么好说的，就是修改日志。CMakeLists.txt和package.xml是标准的ROS包中的标配文件，package.xml描述了包的作者、依赖、版权等信息，CMakeLists.txt则定义了包的编译规则。 launch目录中存放的是各个launch脚本文件，在该目录下我们可以找到在上一章中运行的官方demo脚本文件"demo_backpack_2d.launch"。 urdf目录中则是用URDF文件描述的机器人模型。cartographer_ros目录下才是官方ROS封装的源码，其文件列表如下所示：

我们会在后序的文章中解释这些源码文件，下面让我们来分析一下demo相关的launch脚本。

2. launch脚本

在上一章中我们利用ROS的录包与回放功能加载了官方提前录制好的传感器数据，重现了当时的实验过程。当时在双肩背包中安置了激光雷达和IMU，完成了对德意志博物馆的建图。我们通过roslaunch工具加载脚本"demo_backpack_2d.launch"，其内容如下：

        <launch>
            <param name="/use_sim_time" value="true" />
            <include file="$(find cartographer_ros)/launch/backpack_2d.launch" />
            <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
            <node name="playbag" pkg="rosbag" type="play" args="--clock $(arg bag_filename)" />
        </launch>

这个脚本只是一个通过rosbag实现数据回放的封装，在第五行运行rosbag包的play程序回放数据，第四行通过可视化工具rviz图形化的显示整个建图过程。实际的Cartographer则在第二行通过include标签加载backpack_2d.launch文件。这个launch文件的内容如下所示：

        <launch>
            <param name="robot_description" textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

            <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args="
                    -configuration_directory $(find cartographer_ros)/configuration_files
                    -configuration_basename backpack_2d.lua" output="screen">
                <remap from="echoes" to="horizontal_laser_2d" />
            </node>
            <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
                type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
        </launch>

它除了指定机器人模型之外，就是加载运行了三个节点，维护坐标变换，运行Cartographer系统进行定位和建图。

其中，第2行通过参数"robot_description"指定了机器人的模型文件，这是一个URDF文件描述了实验背包中传感器的安装方式。该模型将被第三行中运行的ROS的程序"robot_state_publisher"，用来维护传感器与背包之间的坐标变换关系。在demo运行的时候，我们可以通过如下的指令查看系统的坐标系，如右图所示。

        $ rosrun rqt_tf_tree rqt_tf_tree

从图中我们可以看到，"robot_state_publisher"维护了水平安装和竖直安装的激光传感器、IMU相对于基座(base link)的坐标变换。 "cartographer_node"则维护了地图(map)、里程计(odom)和机器人基座之间的坐标关系。

脚本backpack_2d.launch的第五到十行中加载了节点"cartographer_node"，它通过一系列参数指定了配置文件，这个节点完成定位和子图的构建。其功能也可以同右图的坐标关系中体现出来。最后在第11和12行加载了"cartographer_occupancy_grid_node"，将子图合并成为占用栅格地图并通过ROS的主题发布出去。

3. CMake

我们已经了解到实际与SLAM相关的节点就是cartographer_node，接下来我们追踪一下编译文件，找到与cartographer_node相关的源文件，为后文继续分析做准备。首先让我们来看一下包cartographer_ros根目录下的CMake文件，在该文件的一开始指定了CMake的版本、工程名称以及依赖项：

cmake_minimum_required(VERSION 2.8.12)
project(cartographer_ros)
set(PACKAGE_DEPENDENCIES
    cartographer_ros_msgs
    ...
    visualization_msgs
)

接着查找包cartographer，并载入其cmake目录下的function.cmake文件，在这个文件中定义了很多以"google_"为前缀的宏(macro)和函数(function)，它们为cartographer提供了一些方便的编译接口。后面的四条语句用于设置和初始化编译环境。

find_package(cartographer REQUIRED)
include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
set(BUILD_SHARED_LIBS OFF)
option(BUILD_GRPC "build features that require Cartographer gRPC support" false)
google_initialize_cartographer_project()
google_enable_testing()

在源码文件的第97行通过添加子目录cartographer_ros来调用该子目录下的CMake文件。在这个文件中具体描述了cartographer_ros中各个节点的编译方式和入口文件。源码文件中的其它语句都是一些查找依赖库，单元测试环境，以及关于包cartographer_ros根目录下子目录launch、urdf、configuration_files的安装说明。

        add_subdirectory("cartographer_ros")

cartographer_ros子目录下的CMake文件就比较单纯了，因为关于编译环境、依赖库等描述已经在根目录下的CMake文件中说明了。下面是我们关注的cartographer_node的编译和安装语句，它在源文件的第28行的位置:

        google_binary(cartographer_node
          SRCS
            node_main.cc
        )
        
        install(TARGETS cartographer_node
          ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
          LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
          RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
        )

它通过调用定义在cartographer包中的function.cmake文件的"google_binary"函数来编译可执行文件，然后使用CMake的install指令来指定安装方式。从第28行到到31行，可以看到cartographer_node的入口main函数定义在node_main.cc中。

4. 完

本文中我们简略的查看了cartographer_ros的目录结构，分析了示例demo的launch脚本发现实际与定位建图相关的节点只有cartographer_node，然后追踪编译系统了解到cartographer_node的入口main函数定义在node_main.cc中。