Zenn
🐷

カチャカでSLAM

2024/12/08に公開
ROS2
slam
tech

ROS 2アドベントカレンダーの8日目の記事になります。
この記事ではカチャカでOSSパッケージを用いて2D SLAMによる地図生成を行うための方法について書いていきます。

方法

前回、カチャカで別途Localizationを行うための方法について記載しました。SLAMを行う際も基本的には同じアプローチで、ros2_brdigeから提供されるmapのtf情報が競合してしまうため、mapのtfが出ないようにros2_bridge側を修正します。修正方法については記事をご覧いただけると幸いです。
https://zenn.dev/ame_b/articles/da4636a0a8048f

cartographerのインストール

今回はcartographerを使用します。
cartographerは大まかにcorrelative/ceresスキャンマッチングによる逐次SLAMとグラフ最適化によるポーズ調整によって構成されます(その他細かいところでは点群のダウンサンプリングなどのpreprocessingやループ検出のモジュールも存在しています)。IMUやOdometryの併用も可ですが、LiDAR単体での地図生成も可能です。また本体自体は出力しないのですが、後処理によって3次元地図の出力も可能です。
今回はLiDAR単体を用いて2Dのグリッドマップを生成します。

想定環境は以下の通りです。
OS : ubuntu 22.04 LTS
ROS distro: ROS 2 Humble

以下の手順でまずcartographerをインストールしていきます。

sudo apt install ros-humble-cartographer ros-humble-cartographer-ros ros-humble-cartographer-rviz

これでひとまずcartographerが使えるようになります。

launchファイルの準備

cartographerを起動するためのlaunchファイルとパラメータファイルを準備します。
ここでは、kachaka-api内に含まれるros 2関連のパッケージは構築済みの前提で進めていきます(~/ros2_ws/src以下にkachaka_bringup等のパッケージが配置されている状態)。

launch

launchファイルはcartographer.launch.pyとして、kachaka_bringup/launchに配置しておきます。

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument

import os

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    share_dir = get_package_share_directory('kachaka_bringup')
    cartographer_ros_dir = get_package_share_directory('cartographer_ros')
    rviz_config_file = os.path.join(
        cartographer_ros_dir, 'configuration_files', 'demo_2d.rviz')
    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir',
                                                  default=os.path.join(share_dir, 'config'))
    configuration_basename = LaunchConfiguration(
        'configuration_basename', default='cartographer.lua')

    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration(
        'publish_period_sec', default='1.0')
    return LaunchDescription([

        Node(package='rviz2',
             executable='rviz2',
             name='rviz2',
             arguments=['-d', rviz_config_file],
             ),

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            output='screen',
            arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'odom']
        ),

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            output='screen',
            arguments=['0.0', '0.0', '0.0', '0.0',
                       '0.0', '0.0', 'odom', 'base_footprint']
        ),

        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            output='log',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-configuration_directory', cartographer_config_dir,
                       '-configuration_basename', configuration_basename],
            remappings=[('scan', '/kachaka/lidar/scan'),]
        ),
        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'),

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

config

設定ファイルはcartographer.luaとして、kachaka_bringup/paramsに配置しておきます。

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 = false,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  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.num_accumulated_range_data = 1

TRAJECTORY_BUILDER_2D.min_range = 0.
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.huber_scale = 1e3

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)

return options

各ファイルに関しては個別パッケージを作成している場合、各々の環境に合わせて適宜配置してください。ファイルを追加したらビルドをお忘れなく。

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to kachaka_bringup

動作確認

実際に起動していきます。
まずはカチャカのros2_bridgeを起動します。

cd kachaka-api/tools/ros2_bridge
./start_bridge.sh  <カチャカのIPアドレス>

続いて、cartographerを起動します。

ros2 launch kachaka_bringup cartographer.launch.py

teleopで操作すると地図が生成されている様子が見れます。

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/kachaka/manual_control/cmd_vel

https://youtu.be/qiovctD1eHM

カチャカではIMU、Odometryトピックも出ているので、cartographer内でフュージョンすることも可です。luaファイル内で以下のようにすると有効化できます。

use_odometry = true,
TRAJECTORY_BUILDER_2D.use_imu_data = true

Discussion