Cartographer安装测试与模块开发(三)--Cartographer在Gazebo仿真环境下的建图以及建图与定位阶段问题（实车也可参考）

参数介绍

之所以要首先介绍参数而不是实操，是因为大部分建图失败、漂移基本上都是参数设置错误引起的，或者说大部分都是TF存在问题，主要是坐标系Frame之间有冲突或者对不上等原因导致的，因此把参数放在前面介绍，了解了参数和对应作用，在对比各类教程设置时才不会碰运气解决报错。

Cartographer的Launch与Lua文件在两个地方存在，注意区分：

//Launch文件位置1：
carto_ws/src/cartographer_ros/cartographer_ros/launch//Lua文件位置1：
carto_ws/src/cartographer_ros/cartographer_ros/configuration_files
carto_ws/src/cartographer/configuration_files//Launch文件位置2：
carto_ws/install_isolated/share/cartographer_ros/launch//Lua文件位置2：
carto_ws/install_isolated/share/cartographer_ros/configuration_files
carto_ws/install_isolated/share/cartographer/configuration_files

如何使用看每个人的习惯，我个人的习惯是调用install_isolated下面的Launch与Lua，使用的时候基于源文件复制一个自己重命名后的Launch与Lua文件来用，且不将source的代码添加到主目录下面的bashrc内，每次使用手动source setup.bash文件。

使用Gazebo来进行建图时，基于demo_backpack_2d.launch文件来实现，打开文件内容如下：

<launch><param name="/use_sim_time" value="true" /><include file="$(find cartographer_ros)/launch/backpack_2d.launch" /><node name="rviz" pkg="rviz" type="rviz" required="true"args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" /><node name="playbag" pkg="rosbag" type="play"args="--clock $(arg bag_filename)" />
</launch>

可以看到文件内主要实现了这样几个功能：

将use_sim_time设置为true，启动了仿真时间

启动了backpack_2d.launch

开启rviz

播放rosbag

继续打开同目录下的backpack_2d.launch，内容如下：

<launch><param name="robot_description"textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" /><node name="robot_state_publisher" pkg="robot_state_publisher"type="robot_state_publisher" /><node name="cartographer_node" pkg="cartographer_ros"type="cartographer_node" args="-configuration_directory $(find cartographer_ros)/configuration_files-configuration_basename backpack_2d.lua"output="screen"><remap from="echoes" to="horizontal_laser_2d" /></node><node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

加载启动了机器人相关的参数和功能

启动了cartographer

remap重映射了激光雷达话题名

启动了cartographer的地图服务

这里因为打算新建一个Launch，所以没必要两个Launch来搞调用，直接将两个内容合成一个Launch就好了，当然需要删掉一些不需要的功能，比如启动了backpack_2d.launch代码不需要了、播放rosbag也不需要、加载启动了机器人相关的参数和功能也删掉（不爱看模型，看坐标系更直观），remap的话题按需修改。

最后合成一个map.launch文件：

<launch><param name="/use_sim_time" value="true" /><node name="cartographer_node" pkg="cartographer_ros"type="cartographer_node" args="-configuration_directory $(find cartographer_ros)/configuration_files-configuration_basename map.lua"output="screen"><remap from="scan" to="scan" /></node><node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" /><node name="rviz" pkg="rviz" type="rviz" required="true"args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

Launch文件准备好之后，继续调整建图的Lua文件，也就是参数部分，在launch文件内加载cartographer的部分可以注意到加载了backpack2d.lua文件。

找到对应文件，Lua文件也和Launch文件一样存在调用其他文件的情况，为了方便起见我新建了一个Lua，把所有调用涉及到的Lua文件全部转移到着一个文件中了，便于调参和管理。

但是需要注意两点：

1、转移Lua文件参数至一个文件中时注意组合的顺序：一般调用关系下越深层的文件在整合时越要靠上，因为它们之间有调用关系。合成一个文件后如果调用段内容在声明段内容的上面是会报错的，如果搞不清楚就按原来的结构改不要整合了。

2、原来的调用关系下，可能有一些参数是重复的：一般是最外层的参数覆盖深层的的参数，这样安排有利于在建图和定位不同功能下都可以调用同样的Lua文件，只需要在最外层把某些值覆写就可以了，整合为一个文件时注意下这个特性。

接下来进入Lua内开始研究参数，这里为了避免歧义依旧按照源代码的Lua文件结构介绍，backpack_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,         --是否需要提供odompublish_frame_projected_to_2d = false,    use_pose_extrapolator = true,use_odometry = false,              --是否使用odomuse_nav_sat = false,use_landmarks = false,num_laser_scans = 0,               --单线激光雷达数num_multi_echo_laser_scans = 1,    --多回波雷达数num_subdivisions_per_laser_scan = 10,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.num_accumulated_range_data = 10return options

其中大部分参数保持默认都不会导致建图失败，最多轻微影响建图效果和速度，但是有几个参数需要重点关注，基本是每个机器都需要根据实际情况调整。

首先是：

num_laser_scans = 0,

num_multi_echo_laser_scans = 1,

两个参数，Cartographer默认使用的雷达不是常见的发出Laserscan的雷达，这一点在第一篇文章跑Demo的时候提到过，因此需要将num_multi_echo_laser_scans改为0，num_laser_scans改为1.（如果只使用一个单线激光雷达的话）

如果没有改这个参数，在运行时会出现大概如下的报错：

队列:等待什么(echo,0)

(记不住原始报错信息了，报错的重点是echo、等待数据、queue之类的字眼)

这个参数设置好后就是坐标系三兄弟：

tracking_frame = "base_link",
published_frame = "base_link",
provide_odom_frame = true,

大部分初学者踩坑都掉在这里了，因为这三个参数如何调整不仅取决于实际仿真环境或者实车环境下发布的TF，而且他们的调整是联动的，先介绍参数具体功能：

tracking_frame要求给出一个坐标系，Cartographer会把机器人其他坐标系转换到这个追踪坐标系，比如雷达坐标系、IMU坐标系等等，这也就要求这个坐标系可以代表整个机器人的运动，通常追踪坐标系可以选择基坐标系或者IMU坐标系，这里将其设置为base_link，因为一般机器人都会有这个坐标系。

published_frame也要求给出一个坐标系，Cartographer会将发布的TF连接在该坐标系上，通常情况是将Map连接到该坐标系上。

provide_odom_frame询问是否需要Cartographer给出odom，如果自己的仿真环境、实车环境带着那当然是false。

注意！以下提到的odom等都为TF中的坐标系，而不是odom话题！

注意点一：如果启动仿真环境或者实车环境后，打印TF树，没有发现tracking_frame赋的值，那么程序是要报错的，可能原因是某些机器人基坐标系是base_footprint或者其他名字，压根没有base_link坐标系，导致其他坐标系没法转移到给定坐标系；或者环境给出了tracking_frame，但是没有发布雷达或者IMU等传感器到该坐标系的TF变换，程序不知道怎么将传感器变换到这个tracking_frame，导致报错。

解决方法也简单，基坐标系问题可以修改tracking_frame的值或者修改机器人urdf的基座标系命名(推荐)，缺TF问题，直接手动给出基座标系到雷达等传感器坐标系的静态TF变换，大部分场景下激光雷达应该都是和机器人之间是固连关系吧？

静态TF发布可以写在仿真环境启动的Launch文件里面,具体内容在第二篇文章有提到。

注意点二：published_frame是Cartographer发布的TF所连接的坐标系，一般Cartographer只提供map的坐标系，因此此处也就是给出map->published_frame.但是需要Cartographer提供odom下，此处就变成了odom->published_frame。通常情况下选择odom都没错，但是这些情况下就需要调整了：

provide_odom_frame为false，也就是不需要提供odom坐标系，同时仿真环境也没有提供odom坐标系，这种情况下需要调整为仿真环境中TF树最高的一个坐标系，以本文第二篇提到的TF树为例，如果没有odom的话，就需要填写base_link，这样最终TF树结构为map->base_link。

provide_odom_frame为true，那么此处需要填写TF树最高的一个坐标系，即Cartographer提供map->odom.我们提供odom->published_frame的信息。

开始建图

调整完参数开始建图，通常话题是/scan、/imu、/odom的都不需要额外修改。

启动仿真环境后，启动Cartographer的Launch，如果出现下面类似的ERROR也不要慌：

[rospack] Error: package 'turtlebot3_sim_test' not found
[librospack]: error while executing command
[ERROR] [1753412286.262920514, 142.830000000]: Could not load resource [package://turtlebot3_sim_test/meshes/bases/waffle_pi_base.stl]: Unable to open file "package://turtlebot3_sim_test/meshes/bases/waffle_pi_base.stl".
[rospack] Error: package 'turtlebot3_sim_test' not found
[librospack]: error while executing command
[ERROR] [1753412286.282568001, 142.850000000]: Could not load resource [package://turtlebot3_sim_test/meshes/sensors/lds.stl]: Unable to open file "package://turtlebot3_sim_test/meshes/sensors/lds.stl".
[rospack] Error: package 'turtlebot3_sim_test' not found
[librospack]: error while executing command
[ERROR] [1753412286.300414295, 142.868000000]: Could not load resource [package://turtlebot3_sim_test/meshes/wheels/left_tire.stl]: Unable to open file "package://turtlebot3_sim_test/meshes/wheels/left_tire.stl".
[rospack] Error: package 'turtlebot3_sim_test' not found
[librospack]: error while executing command
[ERROR] [1753412286.317263047, 142.885000000]: Could not load resource [package://turtlebot3_sim_test/meshes/wheels/right_tire.stl]: Unable to open file "package://turtlebot3_sim_test/meshes/wheels/right_tire.stl".

这是因为仿真环境的功能包放在了Cartographer外面，工作空间不同，也没有在bashrc里面写source，因此模型的加载出现错误，会导致rviz看不见模型，完全不影响功能，反而有利于看TF，好事。

启动后可以看到TF：

接下来就可以操作虚拟环境的小车开始运动来进行建图了。

建图完毕后时用下面的代码保存地图：

1、轨迹停止更新（建图产生的submap都在轨迹0上），这个代码有时执行时会和卡住一样，不要着急，他只有轨迹彻底完成才会输出信息，有的时候建图太慢导致堆积了很多帧没有处理完让他处理就好了

rosservice call /finish_trajectory 0

2、保存地图至指定位置（有不少教程花括号漏了后半截）

rosservice call /write_state "{filename: '<绝对路径>/***.pbstream'}"

3、将Cartographer产生的pbstream地图转化为ROS常用地图格式文件：.yaml和.pgm文件（需要就执行，也可以当作可视化地图用）

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=绝对路径/地图名字 -pbstream_filename=绝对路径/地图名字.pbstream -resolution=0.05

至此建图流程就结束了，下面汇总下可能会出现的问题，以下也有可能在定位阶段出现问题：

问题一：TF类报错，各类TF不存在、父子坐标系冲突、小车建图漂移等等

首先检查Launch中是否启用了虚拟时间，接下来按照上文提到的Lua文件中的坐标三兄弟，检查自己的设置是否正确，检查建议采用关闭Cartographer，先看仿真环境的TF树正不正确，再启动两者结合报错信息查看。

下面给出常用的TF排查命令：

//打印当前TF树结构
rosrun rqt_tf_tree rqt_tf_tree//查看指定两个坐标之间的坐标变换
rosrun tf tf_echo frame1 frame2

问题二：静止状态正常，小车一开始运动rviz中激光帧就开始乱飞或者扭曲变形，具体表现为激光帧乱动、等比例缩小、变为一条线或者一个点。

检查在基坐标系与传感器坐标系的变换，如果使用了静态坐标发布，是否设置在了变化？建议设置为0 0 0 0 0 0也就是重合。如我之前设置了z轴偏移：

<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_base_scan" args="0.0 0.0 0.5 0.0 0.0 0.0 base_link base_scan" />

以上变化导致我的小车在运动时激光帧出现了旋转，变成了垂直于地面，在rvzi中表现为变成一条线。

问题三：建图时空地出现无法去除的灰色或者障碍内部变成白色

障碍内部入侵：

出现灰色区域：

以上主要是参数的问题，可以在Lua文件里面调整击中和未命中的概率，同时也有可能与机器人的速度有关，尤其是机器人转弯速度过快，电脑性能不够（很多使用虚拟机）也有可能影响。

其中障碍内部入侵主要是因为机器人转弯过快且传感器频率、性能不够导致的，一般降低机器人移动速度能有效缓解。

出现灰色区域这一现象主要影响视觉观感，对定位等功能完全不受影响，但如果非常在意，可以找到TRAJECTORY_BUILDER_2D对应Lua文件，调整：

submaps = {
...
...hit_probability = 0.55,miss_probability = 0.46,  --0.49
...
}

其中hit可以让黑色障碍物更快变黑，miss可以让未知区域更快变白，适当降低miss值可以有效降低灰色区域，但是不要降的过低，将会导致障碍也更容易变白，适当降低加上灰色区域多走一步就能有效解决问题。

最后建图如下（80*80M）：