TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解（之二：部署代码解析）

上一篇我们学习了TurtleBot3在ROS2中的SLAM操作命令：

(41条消息) TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解（之一：操作过程）_数据绿洲的博客-CSDN博客

此后我们就开启代码讲解之旅：首先是部署代码部分。

在ROS2中对turtlebot3仿真建图的软件包是：turtlebot3_cartographer。其核心是两个子包：cartographer和cartographer_ros，其中cartographer是SLAM算法本身，而cartographer_ros是cartographer算法与ROS2接口程序包。

今天就来介绍turtlebot3_cartographer软件包是如何部署的？其实，上面提到的cartographer和cartographer_ros软件包是直接与建图相关的软件包，构成cartographer_node节点的核心；要实现建图，还有一个节点也很重要，那就是rviz2。

下一篇将介绍传感器数据驱动。主要介绍TurtleBot3中的传感器（激光雷达和里程计）所收集的数据如何传到地图构建器MapBuilder。

1. turtlebot3_cartographer部署

1.1 部署文件

部署文件位置：

~/tb3_ws/src/turtlebot3/turtlebot3/turtlebot3_cartographer/launch/cartographer.launch.py

从部署文件内容，可以看到turtlebot3_cartographer主要部署两个节点：

- cartographer_node节点：是cartographer_ros包中的一个节点；

- rviz2节点： 是rviz2包中的节点。

部署文件内容如下：

rviz2节点作为可视化节点，在前面机器人建模和环境建模中已经进行过讲解，虽然没有深入到代码级，但是已经能知道其功能和用法，在此就不再赘述。

2. cartographer_node节点

2.1 cartographer_node节点文件入口

cartographer_node节点存在于cartographer_ros软件包中，cartographer_ros的目录结构如下所示：

cartographer_node节点的入口文件是:

cartographer_ros/cartographer_ros/src/node_main.cpp

其由如下CMake文件中定义：

cartographer_ros/cartographer_ros/CMakeLists.txt

2.2. cartographer_node节点的程序入口

cartographer_node的入口文件（cartographer_ros/cartographer_ros/src/node_main.cpp）的main()函数定义如下：

首先进行ROS2 C++客户库rclcpp的初始化，google命令参数解析、logging日志、Flag配置参数解析模块初始化，以及cartographer_ros日志初始化，之后主函数调用了作为cartographer_node的入口Run()函数：cartographer_ros::Run()，其是定义在cartographer_ros命名空间的函数。其主要内容如下：

在入口函数中，首先，它创建了名为“cartographer_node”的rclcpp::Node节点，作为cartographer在ROS2中存在主体。

然后创建坐标变换TF订阅器的缓冲区tf_buffer(tf2_ros::Buffer)：

它将保存10ms的坐标变换消息：

进而创建TF的订阅器tf_listener (tf2_ros::TransformListener)，用于监听坐标变换：

然后，读取cartographer建图所需配置文件，主要包含map_builder和trajectory_builder。该文件由lua语言编写，其位置在：

~/tb3_ws/src/turtlebot3/turtlebot3/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua

在该配置文件中，首先包含了map_builder.lua和trajectory_builder.lua配置文件作为cartographer工作的缺省配置，存在于cartographer软件包中，而本配置文件（turtlebot3_lds_2d.lua）中的配置只是对缺省配置的修改。

 这两个缺省配置文件分别位于：

~/cartographer/configuration_files/map_builder.lua

~/cartographer/configuration_files/trajectory_builder.lua

其中保存有节点配置参数和轨迹配置参数，读出分别存入不同的变量：node_options和trajectory_options。

根据配置参数，接下来就要创建cartographer_node节点中两个非常重要的核心对象：

- 地图构建器map_builder (cartographer::mapping::MapBuilderInterface): 是cartographer算法的核心，主要负责生成地图；

 - 节点管理器node (cartographer_ros::Node): 是cartographer_ros软件包的核心，实现cartographer建图算法与ROS2的接口。

接下来就开始读状态文件，该文件是google ProtoBuff格式，但是初始化时置为空。

最后，根据缺省轨迹配置参数，开始轨迹生成：

cartographer_ros::Node::AddTrajectory()是建图关键入口，具体将在后续的启动轨迹小节中，统一详细介绍。

由此可知，节点管理器封装在cartographer_ros::Node类中，它是ROS2中cartographer建图的主要管理单元，负责管理与建图相关的所有资源，包括ROS2 rclcpp::Node节点及其相关话题发布与订阅、服务、参数管理， 坐标换TF，和cartographer/map_builder等。

2.3. cartographer_ros::Node类

 2.3.1类声明

cartographer_ros::Node类声明中包含了所有管理cartographer建图相关的所有资源，相关结构，处理函数以及服务函数，包括ROS2 rclcpp::Node节点及其相关话题发布与订阅、服务、参数管理， 坐标换TF，和cartographer/map_builder等。

2.3.2 构造函数

首先，将传入的ROS2 rclcpp::Node节点实例保存为成员变量，创建TF广播发布器，以及地图生成器桥实例对象，其中地图生成器桥实例对象是cartographer地图生成器与cartographer_ros节点之间的连接单元，创建地图生成器桥实例对象需要地图生成器和TF缓冲器作为输入参数。

如有需要，注册所有需要的metrics（指标计数器，也不知道翻译）：

接下来，创建后续所需的话题发布器：发布submap_list的发布器，trajectory_node_list发布器，landmark_poses_list发布器，constraint_list发布器，tracked_pose发布器以及点云发布器等6个。当有信息发布时，各自发布器就会用于发布相应的信息：

同时，cartographer_node节点也对外提供一些服务，故需要创建相应的服务器，其中包括submap查询服务器，trajectory查询服务器，start_trajectory服务器，finish_trajectory服务器，write_state服务器，trajectory_state查询服务器，以及metrics查询服务器，共7个。

最后，创建5定时器以便定期向其后续节点发布cartographer_node节点收集到的地图相关信息，其中包括submap_list发布定时器，local_trajectory_data发布定时器，trajectory_node_list发布定时器，landmark_pose_list发布定时器，以及constraint_list发布定时器。

2.4. cartographer_ros::MapBuilderBridge类

由上小节可知，cartographer_node节点管理器（封装在cartographer_ros::Node类中）所发布消息、所提供服务（除Metrics读取服务外）所需要的信息来自cartographer软件包，尤其地图构建器cartographer::mapping::MapBuilderInterface，即cartographer_node中节点中的另一个核心对象。

cartographer_node节点管理器类cartographer_ros::Node中，设计了一个地图构建器桥接器对象map_builder_bridge_ (cartographer_ros::Node::map_builder_bridge_)来负责连接节点中的节点管理器和地图构建器这两个核心对象。它的行为通过桥接类cartographer_ros::MapBuilderBridge来封装，地图构建器类cartographer::mapping::MapBuilderInterface的对象在该桥接类中设计有一个独有智能指针cartographer_ros::MapBuilderBridge::map_builder_来指向。

 2.4.1 类声明

cartographer_ros::MapBuilderBridge类中包含了地图构建器的实例指针，cartographer图节点参数，TF坐标变换缓冲区，传感器桥，以及相关的处理函数，其类声明如下所示：

2.4.2 构造函数

cartographer_ros::MapBuilderBridge构造函数很简单，只是保存了地图构建器的实例指针，以及其他cartographer图节点参数和TF坐标变换缓冲区于类成员变量中。

2.5 cartographer::mapping::MapBuilder类

地图构建器封装cartographer::mapping::MapBuilderInterface 类是一个纯虚类，实际提供服务的是其派生类cartographer::mapping::MapBuilder。

2.5.1 MapBuilder类声明

cartographer::mapping::MapBuilder是cartographer建图的核心，是建图的执行者，其维护了一张机器人行走的位姿图，并基于位姿图通过优化构建地图，其类声明如下所示：

 2.5.2 对象的构造

首先在cartographer_node节点入口（cartographer_ros::Run()）处，创建了MapBuilder对象：

MapBuilder对象是以独有智能指针(unique_ptr)形式存在：

从MapBuilder构造函数可以看出，根据配置的不同MapBuilder可以支持二维也可以支持三维建图，由于cartographer算法是图优化算法，其核心是以机器人位姿为节点、位姿之间距离为边构建位姿图模型，运用位姿图模型构建地图，故构造函数的首要任务就是创建位姿图模型cartographer::mapping::MapBuilder::pose_graph_ （cartographer::mapping::PoseGraph：PoseGraph2D或PoseGraph3D）：

根据前面提到的配置文件(.../turtlebot3/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua)可知，turtlebot3使用的是2D建图。

建图构建器构建地图所需要的信息来自于传感器数据，对多个传感器的数据进行校对的工作被封装在cartographer::mapping::sensor::CollatorInterface类中，该类有两个继承类：TrajectoryCollator和Collator。实例对象cartographer::mapping::MapBuilder::sensor_collator_根据不同的配置以多态形式构建，配置参数也在上述配置文件中，如下所示：

 根据上述配置，turtlebot3机器人传感器校对器使用的是cartographer::mapping::sensor::Collator类对象。

turtlebot3机器人传感器校对器的类定义如下：

至此，ROS2中turtlebot3_cartographer建图所需的主要部件的部署基本完成！！！Cheer！