【ROS2】Cartographerを使ってLiDARの出力だけからSLAMする話

こちらの記事を参考にGazeboから実際のLiDARに置き換えたものになります。
qiita.com

パッケージのインストール

$ sudo apt install ros-humble-cartographer
$ sudo apt install ros-humble-cartographer-rviz
$ sudo apt install ros-humble-laser-filters #/scanトピックの変換のため

起動用パッケージの作成

全部のノードを手動で開いてもいいんですが、流石にターミナルの枚数が多くなりすぎるので起動専用のパッケージを作成します。本当は依存関係を記述するとrosdepで勝手にインストールまでやってくれると思うんですが、難しくてやめました。

$ cd ~/ros2_ws/src
$ ros2 pkg create bringup
$ cd bringup
$ mkdir rviz
$ touch rviz/config.rviz
$ mkdir launch
$ touch launch/bringup.launch.py
$ mkdir config
$ touch config/noimu_lds_2d.lua

最終的な~/ros2_ws/bringup/のディレクトリ構成は以下になります。

~/ros2_ws/src/bringup$ tree
.
├── CMakeLists.txt
├── config
│   └── noimu_lds_2d.lua
├── include
│   └── bringup
├── launch
│   └── bringup.launch.py
├── package.xml
├── rviz
│   └── config.rviz
└── src

6 directories, 5 files

~/ros2_ws/src/bringup/CMakeList.txtの最後に以下を追記します。

install(
  DIRECTORY launch rviz config
  DESTINATION share/${PROJECT_NAME}/
)

~/ros2_ws/src/bringup/launch/bringup.launch.pyに以下を記述します。

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution

def generate_launch_description():
    
    ldlidar_launch = IncludeLaunchDescription(
        launch_description_source=PythonLaunchDescriptionSource([
            get_package_share_directory('ldlidar_node'),
            '/launch/ldlidar_with_mgr.launch.py'
        ])
    )

    laser_filter_node = Node(
            package="laser_filters",
            executable="scan_to_scan_filter_chain",
            parameters=[
                    PathJoinSubstitution([
                    get_package_share_directory("laser_filters"),
                    "examples", "box_filter_example.yaml",
            ])],
            remappings=[('/scan','/ldlidar_node/scan'),('/scan_filtered','/scan')],
    )

    tf2_base_link_ldlidar_base = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'ldlidar_base']
    )

    bringup_dir = get_package_share_directory('bringup')
    cartographer_config_dir = os.path.join(bringup_dir, 'config')
    configuration_basename = 'noimu_lds_2d.lua'

    use_sim_time = False
    resolution = '0.05'
    publish_period_sec = '1.0'

    cartographer_node = 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]
    )

    occupancy_grid_node = Node(
            package='cartographer_ros',
            executable='cartographer_occupancy_grid_node',
            name='cartographer_occupancy_grid_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]
    )

    rviz2_config = os.path.join(
        get_package_share_directory('bringup'),
        'rviz',
        'config.rviz'
    )

    rviz2_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=[["-d"], [rviz2_config]]
    )

    ld = LaunchDescription()
    ld.add_action(ldlidar_launch)
    ld.add_action(laser_filter_node)
    ld.add_action(tf2_base_link_ldlidar_base)
    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    ld.add_action(rviz2_node)
    return ld

~/ros2_ws/src/bringup/rviz/config.rvizには以下のファイルの内容をコピペします。Rvizの設定ファイルは自分で書くと大変なので。
github.com
~/ros2_ws/src/bringup/config/noimu_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_link", --"imu_link",
  published_frame = "base_link", --"odom",
  odom_frame = "odom",
  provide_odom_frame = true, --false,
  publish_frame_projected_to_2d = true,
  use_odometry = false, --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 = true

TRAJECTORY_BUILDER_2D.min_range = 1.0 --0.12
TRAJECTORY_BUILDER_2D.max_range = 8.0 --3.5
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.
TRAJECTORY_BUILDER_2D.use_imu_data = false
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.7

--POSE_GRAPH.optimize_every_n_nodes = 0

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

return options

ROS2でcartographerを使う手順(1) SLAMからPure Localizationまで #Cartographer - Qiita
Cartographer 全パラメータ＆意訳コメント #ROS - Qiita
これらを参考にLD19向けに変更したものです。

ビルドします。

$ cd ~/ros2_ws
$ colcon build --symlink-install --packages-select bringup

実行

$ source install/setup.bash
$ ros2 launch bringup bringup.launch.py

地図の生成と自己位置推定が動作しました。

別のウィンドウで開いたターミナルからrqtグラフを表示してみます

$ rqt_graph

本当はlaser_filtersはいらないんですが、ldlidarの/scanトピックからnamespaceを外すことができなかったため仕方なく挟んでます。Cartographerのparamから直接/ldlidar_node/scanを読み込む設定もあったので気になる方はそちらをいじってみてください。