项目六：ROS2 + Cartographer 2D 激光雷达建图

一、介绍

1.1 概述

• Cartographer是一款可以跨多个平台和传感器配置提供2D和3D实时同步定位和绘图(SLAM)的系统。这个项目提供了Cartographer的ROS集成。
• Cartographer是google推出的一套基于图优化的SLAM算法。Cartographer算法并没有给人惊艳的感觉，但该算法的主要目标是实现低计算资源消耗，达到实时SLAM的目的。
• 该算法主要分为两个部分，第一个部分称为Local SLAM, 该部分通过一帧帧的Laser Scan建立并维护一系列的Submap，而所谓的submap就是一系列的Grid Map。当再有新的Laser Scan中会通过Ceres Scan Matching的方法将其插入到子图中的最佳位置。但是submap会产生误差累积的问题，因此，算法的第二个部分，称为Global SLAM的部分，就是通过Loop Closure来进行闭环检测，来消除累积误差：当一个submap构建完成，也就是不会再有新的laser scan插入到该submap时，算法会将该submap加入到闭环检测中。闭环检测的本质也是一个优化问题，该优化问题被表达成了一个pixel-accurate match的形式，解决优化问题的方法是Branch-and-Bound Approach.
• Cartographer本身是一个C++的库，虽然可以不依赖ROS的环境运行，但为了快速的上手，google还是提供了一个ROS环境下的封装cartographer_ros。
  

1.2 特点

• 工程化代码，依赖少，算法设计精妙，能在嵌入式主控上运行。适合产品级应用；
• 支持 ROS
• 优秀的软件架构设计，便于接入IMU、（单/多线）雷达、里程计、甚至为二维码辅助等视觉识别方式也预留了接口（Landmark）。
• Cartographer支持2D和3D激光雷达的输入，实现机器人定位，并构建栅格地图。
• 2D-SLAM：基于2D栅格地图，可以直接用于导航。
• 3D-SLAM：基于hybridGrid，译为混合概率地图。

1.3 cartographer建图原理

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

因此我们在使用cartographer建图之前，我们需要先看懂这个算法是怎么拿到激光雷达的数据。一般购买雷达会附带相关驱动文件，但不一定适合你的ROS2版本，我使用的雷达是LD14，使用的是自己下载的驱动文件：乐动机器人传感器团队/ldlidar_sl_ros2

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

1.4 资料

Cartographer是一个支持多平台和传感器配置的SLAM的系统。
本文主要讲述在ros2 humble系统上cartographer的安装方法，以及如何配置参数实现自己的ros2 bag包的离线建图。
官方cartographer的github地址：https://github.com/ros2/cartographer
官方cartographer_ros的github地址：https://github.com/ros2/cartographer_ros
官方cartographer说明文档(非ROS2版本)地址：https://google-cartographer.readthedocs.io/en/latest/
官方cartographer_ros说明文档(非ROS2版本)地址：https://google-cartographer-ros.readthedocs.io/en/latest/

二、框架

官方

三、激光雷达底层驱动

3.1 获取激光雷达的ROS2功能包

cd ros2_car_ws/src

git clone  https://github.com/ldrobotSensorTeam/ldlidar_sl_ros2.git

3.2 系统设置

• 第一步，通过板载串口或者USB转串口模块(例如,CH9102模块)的方式使雷达连接到你的系统主板。
• 第二步，设置雷达在系统中挂载的串口设备-x权限(以/dev/ttyACM0为例)
  实际使用时，根据雷达在你的系统中的实际挂载情况来设置，可以使用ls -l /dev命令查看。

如果串口有问题请看这里修改雷达串口：https://alidocs.dingtalk.com/i/p/yr9xmyAdgZWmEOejBYmaEgxWBBaAomNA

资料百度网盘：
链接：https://pan.baidu.com/s/18GjrOzh3jNqX2hnYmWuWwA?pwd=lcz0
提取码：lcz0
–来自百度网盘超级会员V6的分享

cd ~/ros2_car_ws

sudo chmod 777 /dev/ttyACM0

• 第三步，修改launch/目录下雷达产品型号对应的lanuch文件中的port_name值，以ld14.launch.py为例，如下所示。

#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node

'''
Parameter Description:
---
- Set laser scan directon: 
  1. Set counterclockwise, example: {'laser_scan_dir': True}
  2. Set clockwise,        example: {'laser_scan_dir': False}
- Angle crop setting, Mask data within the set angle range:
  1. Enable angle crop fuction:
    1.1. enable angle crop,  example: {'enable_angle_crop_func': True}
    1.2. disable angle crop, example: {'enable_angle_crop_func': False}
  2. Angle cropping interval setting:
  - The distance and intensity data within the set angle range will be set to 0.
  - angle >= 'angle_crop_min' and angle <= 'angle_crop_max' which is [angle_crop_min, angle_crop_max], unit is degress.
    example:
      {'angle_crop_min': 135.0}
      {'angle_crop_max': 225.0}
      which is [135.0, 225.0], angle unit is degress.
'''

def generate_launch_description():
  # LDROBOT LiDAR publisher node
  ldlidar_node = Node(
      package='ldlidar_sl_ros2',
      executable='ldlidar_sl_ros2_node',
      name='ldlidar_publisher_ld14',
      output='screen',
      parameters=[
        {'product_name': 'LDLiDAR_LD14'},
        {'laser_scan_topic_name': 'scan'},
        {'point_cloud_2d_topic_name': 'pointcloud2d'},
        {'frame_id': 'base_laser'},
        {'port_name': '/dev/ttyACM0'},
        {'serial_baudrate' : 115200},
        {'laser_scan_dir': True},
        {'enable_angle_crop_func': False},
        {'angle_crop_min': 135.0},
        {'angle_crop_max': 225.0}
      ]
  )

  # base_link to base_laser tf node
  base_link_to_laser_tf_node = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='base_link_to_base_laser_ld14',
    arguments=['0','0','0.18','0','0','0','base_link','base_laser']
  )


  # Define LaunchDescription variable
  ld = LaunchDescription()

  ld.add_action(ldlidar_node)
  ld.add_action(base_link_to_laser_tf_node)

  return ld

3.3 编译运行

cd ~/ros2_car_ws

colcon build

source install/local_setup.bash

3.4 启动激光雷达节点

• 启动ld14 lidar node

  ros2 launch ldlidar_sl_ros2 ld14.launch.py
  

• 启动ld14 lidar node并显示激光数据在Rviz2上

  ros2 launch ldlidar_sl_ros2 viewer_ld14.launch.py
  

第一条命令运行结果

第二条运行结果

3.5 启动文件

viewer_ld14.launch.py

#!/usr/bin/env python3
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
  # RViZ2 settings
  rviz2_config = os.path.join(
      get_package_share_directory('ldlidar_sl_ros2'),
      'rviz2',
      'ldlidar.rviz'
  )
  
  rviz2_node = Node(
      package='rviz2',
      executable='rviz2',
      name='rviz2_show_ld14',
      arguments=['-d',rviz2_config],
      output='screen'
  )

  #Include LDLidar launch file
  ldlidar_launch = IncludeLaunchDescription(
      launch_description_source=PythonLaunchDescriptionSource([
          get_package_share_directory('ldlidar_sl_ros2'),
          '/launch/ld14.launch.py'
      ])
  )

  # Define LaunchDescription variable
  ld = LaunchDescription()

  ld.add_action(ldlidar_launch)
  ld.add_action(rviz2_node)
  
  return ld

这段代码启动了两个节点：

• RViZ2 节点：这个节点用于显示 RViZ2 界面，并加载特定的配置文件 ldlidar.rviz。
• LDLiDAR 节点：这个节点通过包含另一个 launch 文件 ld14.launch.py 来启动 LDLiDAR。这个 launch 文件位于 ldlidar_sl_ros2 包的 launch 目录中

而ld14.launch.py又定义了两个节点

#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node



def generate_launch_description():
  # LDROBOT LiDAR publisher node
  ldlidar_node = Node(
      package='ldlidar_sl_ros2',
      executable='ldlidar_sl_ros2_node',
      name='ldlidar_publisher_ld14',
      output='screen',
      parameters=[
        {'product_name': 'LDLiDAR_LD14'},
        {'laser_scan_topic_name': 'scan'},
        {'point_cloud_2d_topic_name': 'pointcloud2d'},
        {'frame_id': 'base_laser'},
        {'port_name': '/dev/ttyACM0'},
        {'serial_baudrate' : 115200},
        {'laser_scan_dir': True},
        {'enable_angle_crop_func': False},
        {'angle_crop_min': 135.0},
        {'angle_crop_max': 225.0}
      ]
  )

  # base_link to base_laser tf node
  base_link_to_laser_tf_node = Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='base_link_to_base_laser_ld14',
    arguments=['0','0','0.18','0','0','0','base_link','base_laser']
  )


  # Define LaunchDescription variable
  ld = LaunchDescription()

  ld.add_action(ldlidar_node)
  ld.add_action(base_link_to_laser_tf_node)

  return ld

• LDLiDAR 节点：用于发布激光雷达的数据。
• 静态坐标变换节点：用于定义从 base_link 到 base_laser 的静态变换。

这里，其实大部分都没有什么东西，但是我们需要重点关注的是frame_id这个东西

此时我们查看话题

ros2 topic list

后续利用scan和base_laser来建图。

3.6 frame_id理解理解学习

要看懂这个参数，我们先来看一下ros2中激光雷达的消息接口有一些什么消息即这个接口
Sensor_msgs/msg/LaserScan

使用查看一下接口的消息类型

ros2 interface show sensor_msgs/msg/LaserScan

返回

消息头有三部分的数据，int32 sec,uint32 nanosec,string frame_id，这三个参数的理解就是

sec和nanosec就是时间戳，代表着发布消息的秒和纳秒。
frame_id 是消息中与数据相关联的参考系id，例如在在激光数据中，frame_id对应激光数据采集的参考系(坐标系)。

因此，frame_id就是某一个物体的参考系的坐标名字。

四、cartographer建图过程

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

4.1 安装cartographer

由于ROS2发布了许多的版本，因此，我们在要安装适配于自己版本的包之前可以先查看能适配的包

# 查找符合的包
sudo apt-cache search cartographer

返回了许多与humble有关的包，我们安装这两个

sudo apt install ros-humble-cartographer ros-humble-cartographer-ros
sudo apt install ros-humble-navigation2

等待安装完之后，查看是否有安装好

ros2 pkg list | grep cartogrpaher

返回 即代表安装好

cartographer_ros
cartographer_ros_msgs

在安装完了cartographer之后，其实我们就可以理解成我们在Ubuntu里面安装了一个ROS2的功能包，只是这个功能包并不在我们自己的功能区下面，所以我们只需要启动这个功能包，他就能发布他自己的node和topic。

4.2 .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 执行的记录中提取数据。

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

这里参考的demo_backpack_2d.launch.py和backpack_2d.launch.py;(参考demo_revo_lds.launch.py更好，只用一个文件即可)

ros2 humble cartographer会下载到电脑的路径为 /opt/ros/humble/share/cartographer_ros/launch
参考网址：
cartographer官网

# demo_my_robot_2d.launch.py

def generate_launch_description():
    ## ***** Launch arguments *****
    bag_filename_arg = DeclareLaunchArgument('bag_filename')

    ## ***** Nodes *****
    backpack_2d_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(FindPackageShare('cartographer_ros').find('cartographer_ros') + '/launch/my_robot_2d.launch.py'),
        launch_arguments = {'use_sim_time': 'True'}.items()
        )

    rviz_node = Node(
        package = 'rviz2',
        executable = 'rviz2',
        on_exit = Shutdown(),
        arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'],
        parameters = [{'use_sim_time': True}],
    )

    ros2_bag_play_cmd = ExecuteProcess(
        cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'],
        name = 'rosbag_play',
    )

    return LaunchDescription([
        # Launch arguments
        bag_filename_arg,
        # Nodes
        backpack_2d_launch,
        rviz_node,
        ros2_bag_play_cmd
    ])

# my_robot_2d.launch.py

def generate_launch_description():

    ## ***** Launch arguments *****
    use_sim_time_arg = DeclareLaunchArgument('use_sim_time', default_value = 'False')

    ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    
    '''
    去掉了urdf以及robot_state_publisher_node
    '''

	# 修改为my_robot_2d.lua 文件 以及 激光雷达话题remapping(一个激光雷达的话，映射为scan即可；多个激光雷达需要映射为scan1、scan2...)
    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', 'my_robot_2d.lua'],
        remappings = [
            ('scan', 'scan')],
        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
        
        cartographer_node,
        cartographer_occupancy_grid_node,
    ])

cartographer_node 节点中有一个remap的一个重映射，意思就是将前一个话题的名字重映射为后面的话题名字，就类似于一个改名的操作。
比如我cartographer订阅里程计话题名字odom，但是我发布的话题不是odom，我们可以使用remappings=[('odom', '/odometry/filtered')], 即可将cartographer订阅的话题名映射过去

4.3 .lua文件

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

这里参考的revo_lds.lua;因为只用了激光雷达数据，所以tracking_frame、published_frame设置为雷达的坐标系base_link；

# my_robot_2d.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
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其实就无效了

TF2是ROS2使用的坐标转换的工具

因此，我们其实可以根据我们自己的需求来配置我们所需要的.lua文件

除此之外，我们还可以配置两个文件中的参数，也就是头文件中引入的"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 会将所有数据合并到一起进行建图，生成一个单一的地图。
  } 

trajectory_builder_2d.lua 参数理解

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

TRAJECTORY_BUILDER_2D = {
  use_imu_data = true, --是否使用imu数据
  min_range = 0., --激光的最近距离
  max_range = 30., --激光的最远距离
  min_z = -0.8, --激光的最小高度
  max_z = 2., --激光的最大高度
  missing_data_ray_length = 5., --如果不在min_range和max_range之间，则默认给这个数值
  num_accumulated_range_data = 1, --积累几帧激光数据作为一个Node节点
  voxel_filter_size = 0.025, --一帧激光的网格滤波大小，单位为米。
  
  adaptive_voxel_filter = { --自适应滤波，就是自适应调整滤波网格的大小、使得点云数量刚刚好大于最小点云数量，同时又不过多。
    max_length = 0.5, -- 初始网格滤波的大小，单位为米
    min_num_points = 200, --最小点云数据
    max_range = 50., --点云的最远距离
  },

  loop_closure_adaptive_voxel_filter = { --闭环检测自适应滤波
    max_length = 0.9, --同上
    min_num_points = 100, --同上
    max_range = 50., --同上
  },

  use_online_correlative_scan_matching = false, --使用CSM激光匹配
  real_time_correlative_scan_matcher = { --快速CSN激光匹配
    linear_search_window = 0.1, --平移搜索范围
    angular_search_window = math.rad(20.), --角度搜索范围
    translation_delta_cost_weight = 1e-1, --平移代价权重，就是距离初始值越远，匹配得分要越高才能被信任
    rotation_delta_cost_weight = 1e-1, --旋转代价权重，同上
  },

  ceres_scan_matcher = { --使用ceres优化的方式进行激光匹配
    occupied_space_weight = 1., --占据空间权重
    translation_weight = 10., --平移权重
    rotation_weight = 40., --旋转权重
    ceres_solver_options = { --ceres优化参数
      use_nonmonotonic_steps = false, --使用非单调的步骤，这个功能再明确一下
      max_num_iterations = 20, --最大迭代次数
      num_threads = 1, --使用几个线程优化
    },
  },

  motion_filter = { --移动滤波，就是判断小车有没有动，就是检测2帧激光数据是否相似，要满足一下全部条件
    max_time_seconds = 5., --2帧激光时间戳小间隔
    max_distance_meters = 0.2, --2帧激光最小距离
    max_angle_radians = math.rad(1.), --2帧激光最小角度
  },

  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10., --imu的重力常数
  pose_extrapolator = { --位姿推断
    use_imu_based = false, --3d用来初始化位姿推断器的方式
    constant_velocity = {
      imu_gravity_time_constant = 10., --imu的重力常数
      pose_queue_duration = 0.001,
    },
    imu_based = { --这个还不清楚
      pose_queue_duration = 5.,
      gravity_constant = 9.806,
      pose_translation_weight = 1.,
      pose_rotation_weight = 1.,
      imu_acceleration_weight = 1.,
      imu_rotation_weight = 1.,
      odometry_translation_weight = 1.,
      odometry_rotation_weight = 1.,
      solver_options = {
        use_nonmonotonic_steps = false;
        max_num_iterations = 10;
        num_threads = 1;
      },
    },
  },

  submaps = { --子图
    num_range_data = 90, --子图中Node的数量
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID", --概率栅格地图
      resolution = 0.05, --分辨率
    },
    range_data_inserter = { 
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },
}
# 使用激光建2D时候，一般把use_online_correlative_scan_matching（使用CSM激光匹配）设置为true,

4.4 建图与地图保存

我们修改好上述文件后，即可直接运行

ros2 launch cartographer_ros demo_my_robot_2d.launch.py bag_filename:=/home/bag_filename
#地图的保存
mkdir map
cd map

ros2 run nav2_map_server map_saver_cli -f map

虚拟机建图如下

保存地图需要另一个叫map_saver的节点
在建图完毕之后，我们在我们想要保存地图的地方运行

可以看到已经成功保存下来

注意：在调用map_saver节点之前不要关闭Cartographer节点，不然会丢失地图

会生成.pgm和.yaml两个文件

4.5 节点话题图

# 安装并运行rqt
sudo apt update
# 注意，这里需要安装符合自己ros版本的rqt
# 可以使用sudo apt-cache search 包名字 在apt源里寻找库

sudo apt install ros-humble-rqt*

# 运行
rqt_graph

4.6 问题

在虚拟机上使用是建的图会乱飞，直接使用ubuntu系统不会有这个问题

ubuntu系统建图如下

参考资料

https://blog.youkuaiyun.com/weixin_43087302/article/details/131695391
https://blog.youkuaiyun.com/qq_38880380/article/details/131406336
https://blog.youkuaiyun.com/scarecrow_sun/article/details/127978254