ROS2：如何将Cartographer和Navigation2一起使用（1）：从SLAM到纯定位

有很多文章讲述了ROS2中运行Cartographer建图，但是没有一篇文章进行纯定位导航，因此本文将总结我在ROS2中使用Cartographer从建图到纯定位，再与Nav2结合导航的实现过程。

基本上，用法与ROS1的情况完全相同。

1.操作系统环境

操作系统：Ubuntu22.04LTS ROS2 Humble

2.安装 Cartographer和Navigation2

（1）Cartographer安装，可以参考鱼香ROS的教程：2.Carto介绍及安装 (fishros.com)

sudo apt install ros-humble-cartographer 

（2）安装rviz2插件

sudo apt install ros-humble-cartographer-rviz

（3）安装gazebo插件，鱼香ros安装教程：6.兼容仿真工具-Gazebo (fishros.com)

sudo apt install gazebo

（4）安装navigation2

sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup

3.安装Turtlebot3 Gazebo环境

（1）安装Turtlebot3 ROS2软件包

Turtlebot3-humble源码地址：turtlebot3/turtlebot3_navigation2/launch/navigation2.launch.py at humble-devel · ROBOTIS-GIT/turtlebot3 (github.com)

由于之后需要编辑其中的文件，因此请将其下载到工作区，并使用colcon build构建。

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src/
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
cd ~/ros2_ws/
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
colcon build --symlink-install

（2）安装Turtlebot3 Gazebo软件包

Turtlebot3 Gazebo软件包主要用于在Gazebo中进行仿真，因此请将其下载到工作区。

cd ~/ros2_ws/src/
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/ros2_ws && colcon build --symlink-install

源码地址：ROBOTIS-GIT/turtlebot3_simulations: Simulations for TurtleBot3 (github.com)

也可以使用apt安装它。

sudo apt install ros-humble-turtlebot3-simulations

除了使用Turtlebot3外，也可以使用自己的机器人来进行模拟。例如：使用fishbot或古月居的机器人。Fishbot Github源码：fishros/fishbot: 动手学ROS2第二阶段课程源码|机器人建模仿真fishbot (github.com) 

4.使用Cartographer创建和保存地图

具体可以参考鱼香ROS：3.配置FishBot进行建图 (fishros.com)

（1）Cartographer建图

启动Turtlebot3 gazebo

source ~/ros2_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

turtlebot3_world.launch.py是启动机器人描述文件的.py文件，可以替换成自己机器人的launch文件，例如：fishbot中的fishbot_description中的gazebo.launch.py文件。

启动Cartographer节点

source ~/ros2_ws/install/setup.bash
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

通过teleop_twist_keyboard控制机器人移动

ros2 run teleop_twist_keyboard teleop_twist_keyboard

（2）保存地图

方法一：

ros2 run nav2_map_server map_saver_cli -f ~/map

使用上面的命令保存的地图包括两个文件：.pgm和.yaml。

方法二：保存PBSTREAM文件

通过调用write_state服务，保存.pbstream格式的文件，并写入要保存的路径和文件名。

ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/home/porizou/map.pbstream'}"

5.Cartographer纯定位

（1）创建launch和lua文件

在 ros2_ws/src/turtlebot3/turtlebot3_cartographer/config 中创建一个新的.lua文件以进行纯定位。

include "turtlebot3_lds_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

也可以将turtlebot3_lds_2d.lua替换成Cartographer_ros中的backpack_2d.lua。但是，注意需要将tracking_frame和published_frame替换成适合自己项目的frame。

根据读取上述配置文件和 pbstream 文件的 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 ThisLaunchFileDir


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    turtlebot3_cartographer_prefix = get_package_share_directory('turtlebot3_cartographer')
    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join(
                                                  turtlebot3_cartographer_prefix, 'config'))
    configuration_basename = LaunchConfiguration('configuration_basename',
                                                 default='turtlebot3_lds_2d_localization.lua')

    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')

    rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'),
                                   'rviz', 'tb3_cartographer.rviz')
    pbstream_path = "/home/porizou/map.pbstream"
    return LaunchDescription([
        DeclareLaunchArgument(
            'cartographer_config_dir',
            default_value=cartographer_config_dir,
            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', cartographer_config_dir,
                       '-configuration_basename', configuration_basename,
                       '-load_state_filename', pbstream_path]),
        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(),
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])

（2）执行

启动Turtlebot3 Gazebo

source ~/ros2_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

执行Cartographer节点

source ~/ros2_ws/install/setup.bash
ros2 launch turtlebot3_cartographer cartographer_localization.launch.py use_sim_time:=True

如果顺利，rviz2中将会读取.pbstream文件，并显示之前的映射。

接下来，检查纯定位的操作，适当移动Turtlebot3，比较Gazebo的Turtlebot3和rviz2的TF（base_link)位置，它处于大致相同的位置。

RVIZ2显示

Gazebo显示

 结论

本文中，我们使用Turtlebot3 Gazebo模拟了Cartographer在ROS2环境中的纯定位。下一篇文章中，我们将使用Cartographer纯定位作为定位节点而不是AMCL来运行Navigation2。

参考

ROS 2 Cartographer — ROS 2 workshop documentation (ros2-industrial-workshop.readthedocs.io)

将Cartographer与 ROS2 结合使用的步骤 （1） 从 SLAM 到纯本地化 - 奇塔 (qiita.com)

动手学ROS2 (fishros.com)