ROS2下catographer源码参数解析及工作原理

Flyik

已于 2024-07-09 20:00:51 修改

阅读量1.8k

点赞数 2

分类专栏： ROS2 文章标签： ubuntu

于 2023-03-09 16:58:34 首次发布

原文链接：https://blog.csdn.net/scarecrow_sun/article/details/127978254

版权

ROS2 专栏收录该内容

2 篇文章 2 订阅

订阅专栏

ROS2下catographer源码参数解析及工作原理

1.cartographer初了解

在建图的过程当中，传感器的信息非常重要。在ROS2当中，我们需要使用的传感器信息的方式非常的简单，即订阅传感器topic发布的信息，在接收信息之后，cartographer包会利用其内部的算法对其进行解析构建，进而建图。cartographer整体算法主要依赖的是激光雷达的数据。

因此我们在使用cartographer建图之前，我们需要先看懂这个算法是怎么拿到激光雷达的数据。我的入手点是从商家的激光雷达驱动文件入手进行学习。

整体学习的逻辑顺序如下：

1.激光雷达如何发布数据

2.cartographer如何接收数据

3.基本参数如何配置

4.调试及建图

2.cartographer建图过程

cartographer就是ROS2的一个功能包，和我们自己在ROS2的工作空间下建立的功能包是一个道理。我们如果需要使用这个功能包，其实只需要简单的用launch文件启动这个功能包里面所带有的节点，然后启动我们自己激光雷达的节点，然后cartographer订阅激光雷达节点发布的消息，当然，接收的消息格式和接收的节点的名字都需要我们一开始配置好，也就是使用.lua和.launch.py(在ROS2中)文件。

/cartographer_node节点:

该节点从/scan和/odom话题接收数据进行计算，输出/submap_list数据.

该节点需要接收一个参数配置文件（第二部分写的那个）参数。

/occupancy_grid_node节点：

该节点接收/submap_list子图列表，然后将其拼接成map并发布

该节点需要配置地图分辨率和更新周期两个参数。

那么其实我们需要学习理解的就是catorgrapher的.lua和.launch.py文件是如何配置的，就能够学会怎么运行起来这个算法

3.配置文件

.lua

根据网上给出的建议，我们最好从cartographer官方给出的配置文件进行修改，因此，拿出官网的一个.lua文件进行学习和修改，来看backpack2d.lua。

注：我后面增加了一些其他可以修改的参数，然后我基于官方的backpack2d.lua改成自己需要的配置文件,我的激光雷达的frame_ID是laser


ros2 humble cartographer会下载到电脑的路径为/opt/ros/humble/share/cartographer_ros/configuration_files/

参考网址：

cartographer官网

https://zhuanlan.zhihu.com/p/563264225

backpack2d.lua


include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "laser",
  published_frame = "laser",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 200.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

参数含义:

map_frame：构建地图所使用的坐标系，一般就使用我们前面提到的map即可

tracking_frame： SLAM算法跟踪的帧的ROS帧ID。如果要使用IMU，它应该在这个地方被选用，尽管它可能会漂移。常见的选择是“imu_link”。
tracking_frame一般设置为发布频率最高的传感器的frame_id，cartographer将会把其他数据都转移到该坐标系下进行计算。如果只使用雷达数据进行2D建图，那就只需要将其设置为雷达数据话题的frame_id，一般为laser。如果使用雷达数据+IMU进行2D或者3D建图，因为IMU的发布频率明显高于雷达，所以需要设置为imu数据话题的frame_id，一般imu_link。

published_frame: 要用作发布坐标的子帧的ROS帧ID。例如，如果“odom”框架由系统的不同部分提供，则设置为“odom“。在这种情况下，将发布map_frame中“odom”的坐标。否则，将其设置为“base_link”可能是合适的。
cartographer发布的tf树最后将指向published_frame，即published_frame不是cartographer提供的，这里如果没设置正确，tf树就不能连接成功，建图也就不能正常进行。这个一般设置为底盘的frame_id，也就是URDF文件中的底盘的link name，一般为base_link、base_footprint之类的名字。

odom_frame：仅当provide_odom_frame为true时使用。通常是“odom”。

provide_odom_frame：如果enable, 则local, non-loop-closed, continuous pose 将作为 odom_frame发布在 map_frame.

在大多数情况下，设置 provide_odom_frame 为 true 是有意义的，因为它可以为其他节点提供一个 odom 坐标系，这些节点可以使用机器人的里程计数据来估计机器人的运动，而不必直接处理传感器数据。例如，可以使用 robot_localization 包来融合多个传感器数据，并估计机器人的位姿。
publish_frame_projected_to_2d：如果enable, 则发布姿态将严格限制在纯2D位姿下(不包含roll pitch和z-offset坐标)，这个可以防止出现一些由于pose extrapolation step步骤出现的预期之外不需要的平面外姿态

use_odometry：如果enable,则订阅topic为odom中的nav_msgs/Odometry。这种情况下必须提供里程计.在SLAM过程中也会使用这个消息进行建图。注意:这里如果设置为true，则需要在ROS2节点中发布一个名为odom的topic让carto订阅

use_nav_sat：如果enable, 则订阅主题为fix中的sensor_msgs/NavSatFix。这种情况下必须要使用导航数据

use_landmarks：如果enable，则订阅主题为landmarks中的cartographer_ros_msgs/LandmarkList，必须提供LandmarkLists数据，如1cartographer_ros_msgs/LandmarkEntry中的cartographer_ros_msgs/LandmarkList

num_laser_scans: 要订阅的laser scan的主题数量。为1时，订阅sensor_msgs/LaserScan中的scan主题,或者为多台激光扫描订阅主题的scan_1,scan_2

num_multi_echo_laser_scans: 要订阅的multi-echo laser scan的主题数量，为1时，订阅echoes下的sensor_msgs/MultiEchoLaserScan,或者多个echoes_1, echoes_2

num_subdivisions_per_laser_scan: 将每个接收到的（多回波）激光扫描分成的点云数。细分扫描可以使扫描仪移动时获取的扫描不变形。有一个相应的轨迹生成器选项，可以将细分的扫描累积到一个点云中，用于扫描匹配。若把默认10改为1，1/1=1等于不分割

num_point_clouds: 要订阅的point cloud的主题数量。为1时，订阅points2主题的sensor_msgs/PointCloud2,或者为多台点云订阅主题的points2_1,points2_2

lookup_transform_timeout_sec: 用于使用tf2查找转换的超时秒数。

submap_publish_period_sec: 发布子图姿势的时间间隔（以秒为单位），例如 0.3 秒。

pose_publish_period_sec: 发布姿势的时间间隔（以秒为单位），例如 5e-3 表示频率为 200 Hz。

publish_to_tf: 启用或禁用提供 TF 转换

publish_tracked_pose: 允许将跟踪姿势作为geometry_msgs/PoseStamped 发布到主题“tracked_pose”。

trajectory_publish_period_sec：发布轨迹标记的时间间隔（以秒为单位），例如 30e-3 30 毫秒。

rangefinder_sampling_ratio：测距仪消息的固定比率采样。

odometry_sampling_ratio：里程计消息的固定比率采样。

fixed_frame_sampling_ratio：固定帧消息的固定比率采样。

imu_sampling_ratio IMU： IMU消息的固定比率采样。

landmarks_sampling_ratio：地标消息的固定比率采样。

use_pose_extrapolator： Node里的位姿估计器，作用是融合里程计和IMU，推测出一个位姿。如果use_pose_extrapolator参数为true，发布出的这个位姿不准，因为是先验的位姿，没有经过雷达校准，除非IMU和里程计特别准。因此这个参数一般都是false。如果参数publish_tracked_pose为false，use_pose_extrapolator其实就无效了

除此之外，我们还可以配置两个文件中的参数，也就是头文件中引入的"map_builder.lua" 和 "trajectory_builder.lua"

map_builder.lua 参数理解


include "pose_graph.lua"
 
  MAP_BUILDER = {
   use_trajectory_builder_2d = false, //是否使用2d建图
   use_trajectory_builder_3d = false, //是否使用3d建图
   num_background_threads = 4,            //使用几线程
   pose_graph = POSE_GRAPH,
   collate_by_trajectory = false,        
   //用于控制是否将数据按照轨迹进行分组。

    //如果将 collate_by_trajectory 设置为 true，则 Cartographer 将会按照每个轨迹的 ID 将数据进行分组。在建图过程中，Cartographer 将每个轨迹的数据单独处理，然后将它们合并到最终地图中。这对于多个轨迹的数据进行建图时非常有用。

    //如果将 collate_by_trajectory 设置为 false，则 Cartographer 将忽略轨迹信息，并将所有数据都视为同一个轨迹进行处理。在这种情况下，Cartographer 会将所有数据合并到一起进行建图，生成一个单一的地图。
  }

这其中又含有pose_grapher.lua文件，其中的参数解析参考

cartographer pose_graph.lua 参数解析

trajectory_builder_2d.lua 参数理解

查看这篇网址即可

cartographer trajectory_builder_2d.lua参数备忘

.launch.py文件

在下载了cartographer包之后，我们可以根据launch文件的名字来选择我们需要的文件，命名规则如下

按照功能划分，分为以下几类：

（１）利用已有数据集进行2d/3d建图，如demo_backpack_2d.launch（其又调用了backpack_2d.launch）

（２）利用先验地图及数据集进行全局定位，如demo_backpack_2d_localization.launch

（３）显示pbstream文件

launch文件命名规则标明了其作用：用户根据需要选择launch文件

offline_backpack_2d.launch：离线快速构建全局地图，事先记录的数据集被多倍快速播放
demo_backpack_2d_localization.launch：基于先验地图进行全局定位
demo_backpack_2d.launch：同时定位和建图，需要跑数据包
backpack_2d.launch：同时定位和建图，使用真实的传感器数据
assets_writer_my_robot.launch：用于从.pbstream先前 Cartographer 执行的记录中提取数据。
来源于 https://blog.csdn.net/qq_18276949/article/details/113174339

我们就需要使用launch文件来启动我们的cartographer功能包的节点，同样，我们打开前面.lua文件对应的.launch文件——backpack_2d.launch.py(在ROS2中，由于python语言特性，已经从.launch后缀改为了.launch.py后缀)


from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os

def generate_launch_description():

    ## ***** Launch arguments *****
    # 是否使用仿真时间，真实的机器人我们不需要，设置为False
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    ## ***** File paths ******
    # 找到cartographer功能包的地址
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    ## ***** Nodes *****
    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'backpack_2d.lua'],
        remappings = [
            ('echoes', 'horizontal_laser_2d')],
        output = 'screen'
        )
	
	# 可视化节点
	rviz_node = Node(
          package='rviz2',
          namespace='rviz2',
          executable='rviz2',
          name='rviz2',
          output='screen')

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': True},
            {'resolution': 0.05}],
        )

    return LaunchDescription([
        use_sim_time_arg,
        # Nodes
        rviz_node ,
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

cartographer_node 节点中有一个remap的一个重映射，意思就是将前一个话题的名字重映射为后面的话题名字，就类似于一个改名的操作。

'/configuration_files',


        '-configuration_basename', 'backpack_2d.lua'], 这里的.lua文件为自己配置的.lua文件名字

补充

在进行了融合IMU和里程计数据之后，对carto的使用有了新的理解

对于carto而言，其核心需要使用的数据是激光雷达发布的/scan话题，因此我们一定需要提供/scan话题，并提供对应的数据形式(无论你的数据是激光雷达得到的数据亦或者是其他方式得到的)。而对于carto的使用而言，它订阅数据和frameID是没有任何关系的，对于ROS2里面，拿到数据的方法是订阅topic,因此参数中的map_frame,tracking_frame,published_frame都和carto订阅数据无关，这些frame_ID影响的是其TF树的建立，而与数据订阅无关。

在后续的测试当中，我们发现carto订阅的激光雷达数据就是/scan话题，订阅的里程计数据就是/odom话题，我们暂时还没找到有参数修改能改变其订阅的数据话题名字，比如将其改变为订阅odom_optimize，后续如果继续学习的过程中发现方法，也会继续补充，也欢迎理解的朋友在评论区补充，感谢！

————————————————

原文链接：https://blog.csdn.net/scarecrow_sun/article/details/127978254