亚博microros小车-原生ubuntu支持系列：18 Cartographer建图

Cartographer简介

Cartographer是Google开源的一个ROS系统支持的2D和3D SLAM（simultaneous localization and mapping）库。基于图优化（多线程后端优化、cere构建的problem优化）的方法建图算法。可以结合来自多个传感器（比如，LIDAR、IMU 和摄像头）的数据，同步计算传感器的位置并绘制传感器周围的环境。

cartographer的源码主要包括三个部分：cartographer、cartographer_ros和ceres-solver（后端优化）。

cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图，一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大，但是用submap构建全局地图的长时间过程就会存在很大的累计误差，所以需要利用闭环检测来修正这些submap的位置，闭环检测的基本单元是submap，闭环检测采用scan_match策略。

cartographer的重点内容就是融合多传感器数据（odometry、IMU、LaserScan等）的submap子图创建以及用于闭环检测的scan_match策略的实现。

cartographer_ros是在ROS下面运行的，可以以ROS消息的方式接受各种传感器数据，在处理过后又以消息的形式publish出去，便于调试和可视化。

https://github.com/cartographer-project/cartographer

看完这些还是没啥概念，官方文档吧，

安装：

官网的源码安装比较麻烦。推荐apt直接安装

sudo apt install ros-humble-cartographer

sudo apt install ros-humble-cartographer-ros

安装完了查看下

bohu@bohu-TM1701:~$ ros2 pkg list | grep cartographer
cartographer_ros
cartographer_ros_msgs

官网接下来又到了启动launch脚本的时候了，对于这块还想了解的，推荐看看大佬的文章：

【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现_cartographer navigation真实环境-CSDN博客

大佬的文章讲了很多知识点。

建图

1 启动小车代理，等小车连上

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8090 -v4

2 启动小车处理底层数据程序

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-31-17-14-56-902589-bohu-TM1701-375239
[INFO] [launch]: Default logging verbosity is set to INFO
---------------------robot_type = x3---------------------
[INFO] [complementary_filter_node-1]: process started with pid [375241]
[INFO] [ekf_node-2]: process started with pid [375243]
[INFO] [static_transform_publisher-3]: process started with pid [375245]
[INFO] [joint_state_publisher-4]: process started with pid [375247]
[INFO] [robot_state_publisher-5]: process started with pid [375249]
[INFO] [static_transform_publisher-6]: process started with pid [375251]
[static_transform_publisher-3] [WARN] [1738314897.159059362] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-6] [WARN] [1738314897.165546370] []: Old-style arguments are deprecated; see --help for new-style arguments
[robot_state_publisher-5] [WARN] [1738314897.574904564] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-5] [INFO] [1738314897.575050398] [robot_state_publisher]: got segment base_link
[robot_state_publisher-5] [INFO] [1738314897.575129250] [robot_state_publisher]: got segment imu_Link
[robot_state_publisher-5] [INFO] [1738314897.575148441] [robot_state_publisher]: got segment jq1_Link
[robot_state_publisher-5] [INFO] [1738314897.575162415] [robot_state_publisher]: got segment jq2_Link
[robot_state_publisher-5] [INFO] [1738314897.575176635] [robot_state_publisher]: got segment radar_Link
[robot_state_publisher-5] [INFO] [1738314897.575189851] [robot_state_publisher]: got segment yh_Link
[robot_state_publisher-5] [INFO] [1738314897.575203953] [robot_state_publisher]: got segment yq_Link
[robot_state_publisher-5] [INFO] [1738314897.575216727] [robot_state_publisher]: got segment zh_Link
[robot_state_publisher-5] [INFO] [1738314897.575231399] [robot_state_publisher]: got segment zq_Link
[joint_state_publisher-4] [INFO] [1738314897.812400004] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...

然后，启动rviz，可视化建图，终端输入，

ros2 launch yahboomcar_nav display_launch.py

此时还没运行建图节点，所以没有数据。接下来运行建图节点，终端输入，

ros2 launch yahboomcar_nav map_cartographer_launch.py

启动键盘控制

ros2 run yahboomcar_ctrl yahboom_keyboard

然后控制小车，缓慢的走完需要建图的区域

建图完毕后，输入以下指令保存地图，终端输入，

ros2 launch yahboomcar_nav save_map_launch.py

bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_nav save_map_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-31-17-45-15-428783-bohu-TM1701-377017
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_saver_cli-1]: process started with pid [377019]
[map_saver_cli-1] [INFO] [1738316715.838488933] [map_saver]: 
[map_saver_cli-1] 	map_saver lifecycle node launched. 
[map_saver_cli-1] 	Waiting on external lifecycle transitions to activate
[map_saver_cli-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_saver_cli-1] [INFO] [1738316715.838780993] [map_saver]: Creating
[map_saver_cli-1] [INFO] [1738316715.839511570] [map_saver]: Configuring
[map_saver_cli-1] [INFO] [1738316715.846012025] [map_saver]: Saving map from 'map' topic to '/home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map' file
[map_saver_cli-1] [WARN] [1738316715.846072515] [map_saver]: Free threshold unspecified. Setting it to default value: 0.250000
[map_saver_cli-1] [WARN] [1738316715.846095065] [map_saver]: Occupied threshold unspecified. Setting it to default value: 0.650000
[map_saver_cli-1] [WARN] [1738316715.865849280] [map_io]: Image format unspecified. Setting it to: pgm
[map_saver_cli-1] [INFO] [1738316715.865932329] [map_io]: Received a 208 X 213 map @ 0.05 m/pix
[map_saver_cli-1] [INFO] [1738316715.927583001] [map_io]: Writing map occupancy data to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.pgm
[map_saver_cli-1] [INFO] [1738316715.929668988] [map_io]: Writing map metadata to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.yaml
[map_saver_cli-1] [INFO] [1738316715.929823753] [map_io]: Map saved
[map_saver_cli-1] [INFO] [1738316715.929836633] [map_saver]: Map saved successfully
[map_saver_cli-1] [INFO] [1738316715.932021514] [map_saver]: Destroying
[INFO] [map_saver_cli-1]: process has finished cleanly [pid 377019]

会有两个文件生成，一个是yahboom_map.pgm，一个是yahboom_map.yaml，看下yaml的内容，

image: yahboom_map.pgm

mode: trinary

resolution: 0.05

origin: [-5.56, -1.67, 0]

negate: 0

occupied_thresh: 0.65

free_thresh: 0.25

image：表示地图的图片，也就是yahboom_map.pgm
mode：该属性可以是trinary、scale或者raw之一，取决于所选择的mode，trinary模式是默认模式
resolution：地图的分辨率，米/像素
地图左下角的 2D 位姿(x,y,yaw), 这里的yaw是逆时针方向旋转的（yaw=0 表示没有旋转）。目前系统中的很多部分会忽略yaw值。
negate：是否颠倒白/黑、自由/占用的意义（阈值的解释不受影响）
occupied_thresh：占用概率大于这个阈值的的像素，会被认为是完全占用。
free_thresh：占用概率小于这个阈值的的像素，会被认为是完全自由。

节点通讯图

TF树

源码

src/yahboomcar_nav/launch/display_launch.py

from ament_index_python.packages import get_package_share_pathfrom launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfigurationfrom launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValuedef generate_launch_description():package_path = get_package_share_path('yahboomcar_nav')default_rviz_config_path = package_path / 'rviz/view.rviz'rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),description='Absolute path to rviz config file')rviz_node = Node(package='rviz2',executable='rviz2',name='rviz2',output='screen',arguments=['-d', LaunchConfiguration('rvizconfig')],)return LaunchDescription([rviz_arg,rviz_node])

src/yahboomcar_nav/launch/map_cartographer_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Nodedef generate_launch_description():package_launch_path =os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch')cartographer_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource([package_launch_path, '/cartographer_launch.py']))base_link_to_laser_tf_node = Node(package='tf2_ros',executable='static_transform_publisher',name='base_link_to_base_laser',arguments=['-0.0046412', '0' , '0.094079','0','0','0','base_link','laser_frame']) return LaunchDescription([cartographer_launch,base_link_to_laser_tf_node])

src/yahboomcar_nav/launch/cartographer_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDirdef generate_launch_description():use_sim_time = LaunchConfiguration('use_sim_time', default='false')package_path = get_package_share_directory('yahboomcar_nav')configuration_directory = LaunchConfiguration('configuration_directory', default=os.path.join(package_path, 'params'))configuration_basename = LaunchConfiguration('configuration_basename', default='lds_2d.lua')resolution = LaunchConfiguration('resolution', default='0.05')publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')return LaunchDescription([DeclareLaunchArgument('configuration_directory',default_value=configuration_directory,description='Full path to config file to load'),DeclareLaunchArgument('configuration_basename',default_value=configuration_basename,description='Name of lua file for cartographer'),DeclareLaunchArgument('use_sim_time',default_value='false',description='Use simulation (Gazebo) clock if true'),Node(package='cartographer_ros',executable='cartographer_node',name='cartographer_node',output='screen',parameters=[{'use_sim_time': use_sim_time}],arguments=['-configuration_directory', configuration_directory,'-configuration_basename', configuration_basename],remappings=[('/odom','/odom')]),DeclareLaunchArgument('resolution',default_value=resolution,description='Resolution of a grid cell in the published occupancy grid'),DeclareLaunchArgument('publish_period_sec',default_value=publish_period_sec,description='OccupancyGrid publishing period'),IncludeLaunchDescription(PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid_launch.py']),launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,'publish_period_sec': publish_period_sec}.items(),),])

src/yahboomcar_nav/launch/occupancy_grid_launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfigurationdef generate_launch_description():use_sim_time = LaunchConfiguration('use_sim_time', default='false')resolution = LaunchConfiguration('resolution', default='0.05')publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')return LaunchDescription([DeclareLaunchArgument('resolution',default_value=resolution,description='Resolution of a grid cell in the published occupancy grid'),DeclareLaunchArgument('publish_period_sec',default_value=publish_period_sec,description='OccupancyGrid publishing period'),DeclareLaunchArgument('use_sim_time',default_value='false',description='Use simulation (Gazebo) clock if true'),Node(package='cartographer_ros',executable='cartographer_occupancy_grid_node',name='occupancy_grid_node',output='screen',parameters=[{'use_sim_time': use_sim_time}],arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]),])

cartographer_launch.py 里面调用了lds_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_footprint",published_frame = "odom",--发布map到odom之间的位姿态odom_frame = "odom",provide_odom_frame = false,publish_frame_projected_to_2d = false,use_odometry = true,--使用里程计数据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 = trueTRAJECTORY_BUILDER_2D.use_imu_data = false  --是否使用IMU数据
TRAJECTORY_BUILDER_2D.min_range = 0.10 --激光的最近有效距离
TRAJECTORY_BUILDER_2D.max_range = 3.5  --激光最远的有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.  --无效激光数据设置距离为该数值
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true --是否使用在线相关扫描匹配
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)  --运动敏感度POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7return options

以下参数来自大佬：【10天速通Navigation2】(三) ：Cartographer建图算法配置：从仿真到实车，从原理到实现_cartographer navigation真实环境-CSDN博客

options: 这是一个表（table），包含了Cartographer算法的各种配置选项。

map_builder: 指定用于构建地图的类。

trajectory_builder: 指定用于构建轨迹的类。

map_frame: 地图的坐标系。
tracking_frame: 跟踪的坐标系，通常是机器人底盘的坐标系。
published_frame: 发布的坐标系，通常是用于导航的坐标系。注意这里用于设置cartographer是否发布从tracking_frame到odom_frame之间的tf树

odom_frame: 里程计坐标系。
provide_odom_frame: 是否提供里程计坐标系。
publish_frame_projected_to_2d: 是否将3D数据投影到2D进行发布。
use_pose_extrapolator: 是否使用姿态外推器。
==use_odometry: 是否使用里程计数据。==通常我们需要引入odom或者IMU用于辅助定位,否则会出现偏移和无法闭环的问题(后面我们会讲到)
use_nav_sat: 是否使用导航卫星数据。
use_landmarks: 是否使用地标数据。
num_laser_scans: 使用的激光扫描仪数量。
num_multi_echo_laser_scans: 使用多回声激光扫描仪的数量。
num_subdivisions_per_laser_scan: 每个激光扫描数据的细分数量。
num_point_clouds: 使用的点云数据数量。
lookup_transform_timeout_sec: 查找变换的超时时间。
submap_publish_period_sec: 发布子地图的周期。
pose_publish_period_sec: 发布姿态的周期。
trajectory_publish_period_sec: 发布轨迹的周期。
rangefinder_sampling_ratio: 激光采样比率。
odometry_sampling_ratio: 里程计采样比率。
fixed_frame_pose_sampling_ratio: 固定帧姿态采样比率。
imu_sampling_ratio: IMU采样比率。
landmarks_sampling_ratio: 地标采样比率。