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包中的节点。

部署文件内容如下:

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第1张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第2张图片

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

2. cartographer_node节点

2.1 cartographer_node节点文件入口

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第3张图片

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()函数定义如下:

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第4张图片

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第5张图片

在入口函数中,首先,它创建了名为“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格式,但是初始化时置为空。

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第6张图片

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等。

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第7张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第8张图片TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第9张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第10张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第11张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第12张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第13张图片

2.3.2 构造函数

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第14张图片

如有需要,注册所有需要的metrics(指标计数器,也不知道翻译):

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第15张图片

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第16张图片

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第17张图片

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坐标变换缓冲区,传感器桥,以及相关的处理函数,其类声明如下所示:

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第18张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第19张图片TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第20张图片

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建图的核心,是建图的执行者,其维护了一张机器人行走的位姿图,并基于位姿图通过优化构建地图,其类声明如下所示:

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第21张图片

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第22张图片

 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在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第23张图片

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

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第24张图片

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

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

TurtleBot3在ROS2 humble中的仿真解析之建图: SLAM操作与代码讲解(之二:部署代码解析)_第25张图片

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

你可能感兴趣的:(机器人,ROS2,自动驾驶,机器人,人工智能)