Cartographer源码阅读2：0基础小白也能上手 跑自己的机器人

为了先开始帮组里干活，我先开始学习如何用自己的机器人跑算法部分。

参照官方文档，部分参照大佬Cartographer 建图教程 | Vision Knowledge Base的博客

Cartographer 具体参数配置

创建 .lua 配置

Cartographer可以配置在各种机器人上工作，每个机器人的配置必须从Lua脚本定义的数据结构读取，所以我们需要配置不同的.lua文件来对应不同的机器人。

官方示例配置在：
src/cartographer_ros/cartographer_ros/configuration_files 和
install_isolated/share/cartographer_ros/configuration_files/ 中。

我们可以复制其中一个示例，然后根据自己的需要进行调整：

具体配置可参考 Lua 配置参考文档以及Cartographer配置值。同时我也将部分附在文末。

2D：

cp install_isolated/share/cartographer_ros/configuration_files/backpack_2d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua

3D：

cp install_isolated/share/cartographer_ros/configuration_files/backpack_3d.lua install_isolated/share/cartographer_ros/configuration_files/my_robot.lua

创建 .launch 配置：

官方示例 .launch 文件在src/cartographer_ros/cartographer_ros/configuration_files 和 install_isolated/share/cartographer_ros/configuration_files/ 中。同样可以先复制官方提供的示例：

cp install_isolated/share/cartographer_ros/launch/backpack_3d.launch install_isolated/share/cartographer_ros/launch/my_robot.launch
cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d.launch install_isolated/share/cartographer_ros/launch/demo_my_robot.launch
cp install_isolated/share/cartographer_ros/launch/offline_backpack_3d.launch install_isolated/share/cartographer_ros/launch/offline_my_robot.launch
cp install_isolated/share/cartographer_ros/launch/demo_backpack_3d_localization.launch install_isolated/share/cartographer_ros/launch/demo_my_robot_localization.launch
cp install_isolated/share/cartographer_ros/launch/assets_writer_backpack_3d.launch install_isolated/share/cartographer_ros/launch/assets_writer_my_robot.launch

• my_robot.launch旨在用于机器人，以使用真实传感器数据在线（实时）执行 SLAM。
• demo_my_robot.launch旨在从开发计算机使用，并需要一个参数来重播记录中的数据。此启动文件还会生成一个 rviz 窗口，该窗口配置为可视化 Cartographer 的状态。bag_filename
• offline_my_robot.launch与 SLAM 非常相似，但会尝试尽快执行 SLAM。这可以显著提高地图构建速度。此启动文件还可以使用提供给参数的多个 bag 文件。demo_my_robot.launchbag_filenames
• demo_my_robot_localization.launch与上一个 Cartographer 执行的记录非常相似，但需要一个指向先前 Cartographer 执行记录的参数。之前的记录将用作预先计算的地图，Cartographer 将仅在此地图上执行定位。demo_my_robot.launchload_state_filename.pbstream
• assets_writer_my_robot.launch用于从先前 Cartographer 执行的记录中提取数据。.pbstream

我们需要对这些文件进行一些调整以适合我们的机器人。

• 给定的每个参数 to 都应该调整为指向 -configuration_basenamemy_robot.lua
• 如果决定使用 robot 的描述，则应将描述放入并调整参数以指向文件名。.urdfinstall_isolated/share/cartographer_ros/urdfrobot_description
• 如果您决定使用messages，则可以删除参数、节点和表示 ./tfrobot_descriptionrobot_state_publisher-urdf
• 如果包或传感器发布的主题名称与 Cartographer ROS 预期的主题名称不匹配，可以使用元素来重定向主题。预期的主题名称取决于使用的范围查找设备的类型。<remap>

 每个 .launch 文件的具体使用方法可查看这里

都设置好之后，可以启动carto来试用配置 

roslaunch cartographer_ros my_robot.launch bag_filename:=/path/to/your_bag.bag

 运行自己的 Cartographer ROS

注意:1.不要在 src 下写自己的代码，不然重新编译时可能会失败。官方教程是在install_isolated/share 文件夹下修改代码。2.默认Cartographer 的工作空间为 catkin_ws,自行修改。3.雷达功能包lsn10自行修改

2D 建图 + 激光雷达​

1. 在 Cartographer ROS 中使用雷达

在 Cartographer 的工作空间下编译 lsn10 功能包。

将雷达的功能包放至 catkin_ws/src/cartographer_ros 下, 。

运行以下命令，编译新的工作空间：

# 重新编译工作空间
# 如果碰到报错，删除所有已编译出来的文件，再次编译
catkin_make_isolated --install --use-ninja

看到 Finish 字眼说明编译成功。 

2. 创建 .lua 配置文件

运行下列命令，复制 revo_lds.lua 文件作为自己的新的 .lua 配置文件。

# 进入 Cartographer 工作空间
cd catkin_ws
# 复制2D建图 revo_lds.lua 文件  
cp install_isolated/share/cartographer_ros/configuration_files/revo_lds.lua install_isolated/share/cartographer_ros/configuration_files/my_robot_laser_only.lua

将参数 tracking_frame 修改成雷达的frame_id，即 laser。

将参数 published_frame 修改成底盘的frame_id，即 base_link。

将参数 provide_odom_frame 设置为 false。
没有用到里程计，所以将其设置成 false。

tracking_frame: SLAM 算法跟踪的帧的 ROS 帧 ID。
​tracking_frame 一般设置为发布频率最高的传感器的 frame_id，Cartographer 将会把其他数据都转移到该坐标系下进行计算。

published_frame: 要用作发布坐标的子帧的 ROS 帧 ID。
如果 odom 框架由系统的不同部分提供，则设置为 odom。
在这种情况下将发布 map_frame 中 odom 的坐标。否则，将其设置为 base_link 可能是合适的。

具体代码如下：

tracking_frame = "laser"
published_frame = "base_link"

provide_odom_frame = false

将下列参数 TRAJECTORY_BUILDER_2D.min_range , TRAJECTORY_BUILDER_2D.max_range 修改成你所用的雷达的具体扫描参数。

具体代码如下：

TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 11.

3. 创建 .launch 配置文件

运行下列命令，复制 demo_revo_lds.launch 文件作为自己的新的 .launch 配置文件。

# 进入 Cartographer 工作空间
cd catkin_ws
# 复制2D建图 demo_revo_lds.launch 文件
cp install_isolated/share/cartographer_ros/launch/demo_revo_lds.launch install_isolated/share/cartographer_ros/launch/my_robot_laser_only.launch

修改 cartographer_node 节点，映射雷达的 topic，即 /scan。

将 rviz 节点切换至你所用的 .rviz 文件。

关闭运行 bag 的节点。

增加 map 与 laser 的 tf 关系节点。

具体代码如下：

<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 my_robot_only_laser.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 pkg="tf2_ros" type="static_transform_publisher" name="laser_broadcaster" 
      args="0 0 0 0 0 0 map laser" />

  <!-- 请修改成自己的 rviz 路径 -->      
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="/home/zr/.ros/laser.rviz" />
</launch>

4. 合并雷达的功能包与 Cartographer ROS

将 lsn10 节点写进上述 .launch 文件。

节点代码如下：

  <include file="$(find lsn10)/launch/lsn10.launch" />

整体 my_robot_laser_only.launch 代码如下：

<launch>
  <include file="$(find lsn10)/launch/lsn10.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 my_robot_only_laser.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 pkg="tf2_ros" type="static_transform_publisher" name="laser_broadcaster" 
      args="0 0 0 0 0 0 map laser" />
      
  <!-- 请修改成自己的 rviz 路径 -->      
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="/home/zr/.ros/laser.rviz" />
</launch>

5. 运行建图程序

运行以下命令，启动建图程序。

# 进入 Cartographer 工作空间
cd catkin_ws
# 不能编译！否则刚刚写的代码就没了！！
# 原因为 src 里没有改动，编译的话会将 install_isolated恢复成初始状态。
# 常规，source 工作空间环境
source install_isolated/setup.bash 
# 运行程序
roslaunch cartographer_ros my_robot_laser_only.launch

6. 在 rviz 中添加 topic

添加 Submaps 话题即可看到雷达建图。 

其他 topic 可自行添加。

主要使用的 topic 为 Map 、TF、LaserScan 、MarkArray 。

最终运行效果如下： 

保存已建好的地图​

使用 Cartographer 建图后需要在工作空间输入以下命令进行保存地图。

1. 停止建图

# 停止地图构建
rosservice call /finish_trajectory 0

2. 保存地图

# 保存地图
rosservice call /write_state "{filename: '<绝对路径>/‘<地图名字>’.pbstream'}"

# 如：
rosservice call /write_state "{filename: ${HOME}/catkin_ws/map/map.pbstream}"

3. 将 Cartographer 保存的地图转换为 ROS 格式下的栅格地图

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

# 如：
rosrun cartographer_ros cartographer_pbstream_to_ros_map -pbstream_filename=${HOME}/catkin_ws/map/map.pbstream -map_filestem=${HOME}/catkin_ws/map/map>

为了更加方便地保存地图，还可以将这些命令做成一个脚本文件 CartographerSaveMap.sh。​

脚本文件 CartographerSaveMap.sh 的代码如下：

#!/bin/bash
# enter the workspace
source install_isolated/setup.bash

# map  folder 
map_dir="${HOME}/catkin_ws/map"      
 # map name defined as current datatime   
map_name=$(date +%Y%m%d_%H-%M-%S)  

# check the file folder exist
if [ ! -d "$map_dir" ];then
  echo "文件夹不存在, 即将创建文件夹"
  mkdir -p $map_dir
fi

# finish slam
rosservice call /finish_trajectory 0

# make pbstream
rosservice call /write_state "{filename: '$map_dir/$map_name.pbstream'}"

# pbstream to map
rosrun cartographer_ros cartographer_pbstream_to_ros_map \
-pbstream_filename=$map_dir/$map_name.pbstream \
-map_filestem=$map_dir/$map_name

给脚本赋予权限:

  sudo chmod 777 CartographerSaveMap.sh

运行脚本文件即可保存地图。

文末为Lua配置参考。

Lua配置参考：

map_frame

用于发布子图的 ROS 帧 ID，姿势的父帧， 通常是 “map”。

tracking_frame

SLAM 算法跟踪的帧的 ROS 帧 ID。如果 IMU ，则它应该位于其位置，尽管它可能会旋转。常见的 choice 为 “imu_link”。

published_frame

用作发布姿势的子帧的 ROS 帧 ID。例如 如果 “odom” 帧由系统的不同部分提供，则为 “odom”。在 在这种情况下，map_frame中 “Odom” 的姿势将被公布。否则 将其设置为 “base_link” 可能是合适的。
​
odom_frame

仅当 provide_odom_frame 为 true 时使用。published_frame 和 map_frame 之间的帧，用于发布（非闭环）本地 SLAM 结果。通常是 “odom”。

provide_odom_frame

如果启用，则本地非循环闭合的连续姿势将发布为 map_frame中的odom_frame。

publish_frame_projected_to_2d

如果启用，则发布的姿势将被限制为纯 2D 姿势（无滚动、 pitch 或 z 偏移量）。这可以防止 2D 模式（例如，如果姿势 应作为“基本足迹”式框架发布）

use_odometry

如果启用，则订阅主题 “odom” 上的 nav_msgs/Odometry。里程计 必须提供，并且该信息将包含在 SLAM 中。

use_nav_sat

如果启用，则订阅主题 “fix” 的 sensor_msgs/NavSatFix。 在这种情况下，必须提供导航数据，并且信息将是 包含在全局 SLAM 中。

use_landmarks

如果启用，则订阅主题上的 cartographer_ros_msgs/LandmarkList “地标”。必须提供地标，如 cartographer_ros_msgs/LandmarkList 中的 cartographer_ros_msgs/LandmarkEntry。如果提供了 cartographer_ros_msgs/LandmarkEntry 数据，则信息 将根据 cartographer_ros_msgs/LandmarkEntry 的 ID 包含在 SLAM 中。cartographer_ros_msgs/LandmarkList 应以与其他传感器相当的采样率提供。该列表可以为空，但必须提供，因为 Cartographer 对传感器数据进行严格时间排序，以使地标具有确定性。但是，可以将 trajectory builder 选项 “collate_landmarks” 设置为 false，并允许使用非确定性但也是非阻塞的方法。

num_laser_scans

要订阅的 laser scan 主题数。订阅 sensor_msgs/LaserScan 关于一个激光扫描仪的 “scan” 主题或主题 “scan_1”、“scan_2”等，用于多个激光扫描仪。

num_multi_echo_laser_scans

要订阅的多回波激光扫描主题的数量。订阅 sensor_msgs/MultiEchoLaserScan 关于一台激光扫描仪的“echoes”主题， 或多个激光扫描仪的主题 “echoes_1”、“echoes_2” 等。

num_subdivisions_per_laser_scan

将每个接收的（多回波）激光扫描拆分成的点云数。 细分扫描可以取消翘曲获取的扫描，而 扫描仪在移动。有一个相应的 trajectory builder 选项，用于 将细分的扫描累积到将用于扫描的点云中 匹配。

num_point_clouds

要订阅的点云主题数。订阅 sensor_msgs/PointCloud2 的 “points2” 主题，用于一台测距仪，或 多个测距仪的主题 “points2_1”、“points2_2” 等。

lookup_transform_timeout_sec

超时（以秒为单位），用于使用 tf2 查找转换。

submap_publish_period_sec

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

pose_publish_period_sec

发布姿势的间隔（以秒为单位），例如 5e-3 的频率 200 赫兹。

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 消息的固定比率采样。

landmarks_sampling_ratio

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

​