SLAM (自己位置推定) のための Localization ~slam 設定調整~ (ROS2-Foxy)

はじめに
前提条件
SLAM 設定ファイルとlaunch ファイルの確認
slam_toobox を Rviz2 上で確認
おわりに

はじめに

前回は ekf の設定方法について説明しました。
非常に多くの設定項目がありますが、用途に合わせて変更してください。

今回は、slam_toolbox の設定内容について説明していきます。非常に多いのですが、基本はデフォルトでうまく動いてくれる非常に強力なライブラリです。

前提条件

前提条件は、以下の通りです。

first_slam_settings.yaml を使用して Localization ができる。

SLAM 設定ファイルとlaunch ファイルの確認

まず始めに、first_slam.launch.py を確認します。

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

def generate_launch_description():
    package_dir = get_package_share_directory("ros2_first_test")
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    slam_params = os.path.join(package_dir, "config", 'first_slam_settings.yaml')
    
    return LaunchDescription([
        Node(
            parameters=[
            {"slam_params_file": slam_params},
            {'use_sim_time': use_sim_time}
            ],
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            name='slam_toolbox',
            output='screen'),
    ])

slam_toolbox の、async_slam_toolbox_node を使用します。
詳細な github はこちらにあります。

slam_settings.yaml は以下のコードになります。

slam_toolbox:
  ros__parameters:

    # Plugin params
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: scan
    mode: mapping #localization

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0 #if 0 never publishes odometry
    map_update_interval: 1.0
    resolution: 0.05
    max_laser_range: 20.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: true
    use_scan_barycenter: true
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.5
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true 
    loop_match_minimum_chain_size: 10           
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5      
    angle_variance_penalty: 1.0    

    fine_search_angle_offset: 0.00349     
    coarse_search_angle_offset: 0.349   
    coarse_angle_resolution: 0.0349        
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

中身について説明していきます

# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None

solver は github を確認しましたが、今のところ Ceres しかないようです。

# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: scan
mode: mapping #localization

frame や topic は、ご使用の環境に合わせてください。今回は上記の構成で進めます。
mode に関しては、mapping しか確認できませんでした。

map_update_interval: 1.0

map_update_interval は、/map トピックへパブリッシュする間隔を指定します。早く結果が見たい方は 1.0 で、お使いの環境の処理能力に合わせてください。

do_loop_closing: true

ループ閉じ込みをするかどうかを選択します。基本は true の方がマップの精度が上がります。

slam_toobox を Rviz2 上で確認

slam_toolbox を Rviz2 上で確認する方法を説明します。

まずは、localization と slam の launch ファイルを別々のターミナルで起動します。

ros2 launch ros2_first_test first_localization.launch.py

ros2 launch ros2_first_test first_slam.launch.py

Rviz2 上で /map を表示してから、以下の赤枠の Add New Panel を選択します。

以下のプロパティが表示されるので、SlamToolboxPlugin を選択します。

こちらを選択すると、Rviz2 に slam_toolbox のパネルが表示されます。

このパネルから、 Save Map で map の保存ができるようになります。

保存先は、slam_toolbox を起動したときのカレントディレクトリになります。

おわりに

今回は slam_toolbox の使用方法や確認方法について説明しました。
本当は yaml ファイル全てを説明したかったのですが、github から解説を探すことができなかったので、C++ から探す必要がありそうです…。

次回は、ついにナビゲーションについて説明していきます。Path-Planning とも言われますが、障害物を避けつつ、経路探索ができるようになります。
ここまでできれば、AGV の完成ですね。