はじめに

　今回から2つの記事にまたがって、ROS 2において、対向二輪型ロボットに自動走行を行わせる方法について記載します。今回の記事では、自動走行に必要な環境地図の作成を、slam_toolboxを用いて進めていきます。
　使用するロボットや環境については、前回作成したものを一部流用しています。

この記事はROSに関する投稿の一部です。
目次はこちら

前提条件

　今回の記事は以下の環境で動かすこと前提に記載しています。

	条件
OS	Ubuntu 22.04
ROS	ROS 2 humble

準備

　まずは、前回の記事で作成したURDFファイルに、Gazeboにおける色と摩擦の設定を追加します。以下を<robot> ~ </robot>の中に入れてください。

~/ros2_ws/src/sim_py_01/urdf/wheel_robot_simple.urdf(一部のみ)

<gazebo reference="body_link">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Gray</material>
</gazebo>

<gazebo reference="back_ball_link">
  <mu1>0.0</mu1>
  <mu2>0.0</mu2>
  <material>Gazebo/Gray</material>
</gazebo>

<gazebo reference="left_wheel_link">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Red</material>
</gazebo>

<gazebo reference="right_wheel_link">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Red</material>
</gazebo>

<gazebo reference="front_laser_link">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Blue</material>
</gazebo>

追加した設定

<mu1> ... 摩擦のピラミッドモデルにおける第1方向に沿った摩擦係数。詳細は下記
<mu2> ... 摩擦のピラミッドモデルにおける第2方向に沿った摩擦係数。ほとんどの場合<mu1>と同じ値になる。詳細は下記
<material> ... Gazebo上で表示する色。gazebo.materialで定義されているものが使用可能

その他追加可能な設定

<fdir1> ... 摩擦のピラミッドモデルにおける第1方向を示す3次元の単位ベクトル。デフォルトは0 0 0であり、衝突し合う2つの物体の<fdir1>の要素が全て0である場合、ワールド座標に揃えられる(ワールド座標のどの方向に揃えられるかは未確認)。詳細は下記

Gazeboにおける摩擦について

　Gazeboにおける摩擦は、摩擦円錐(Friction Cone)を四角錐に近似して表現されています。
　摩擦円錐とは、物体が滑らない力の範囲を示す図のことです。円錐の高さは物体にかかる垂直抗力の大きさを表し、地面等の接触面から広がる円錐の形をしています。この範囲外を向くベクトルの力が働くと、物体は滑りながら動きます。
　Gazeboの摩擦四角錐には、3次元の単位ベクトルfdir1と、長さを示すパラメータmu1およびmu2が設定可能です。fdir1はmu1の方向を示し、mu1とmu2は互いに垂直の関係にあります。そして、mu1とmu2を2倍したものを対角線とするひし形が、四角錐の底面となります。

摩擦円錐

ビルド

　以下のコマンドを実行してsim_py_01パッケージをビルドします。

cd ~/ros2_ws
colcon build --packages-select sim_py_01
source ~/ros2_ws/install/setup.bash

設定を追加する前と追加した後の比較

　上記の設定を追加する前と追加した後の違いを以下に表します。
　色の設定を追加したことで、Gazebo上のロボットに色が付いたことを確認できます。
　また、ロボットに前進を行う命令を送ると、摩擦の設定を追加する前は摩擦力が$0$のため、ロボットは前に進みながら横に滑ってしまいます。しかし、摩擦の設定を追加した後は、横に滑ることなくしっかりと前進できていることが確認できます。

設定を追加する前での前進	設定を追加した後での前進

slam_toolboxを用いた地図作成

　ロボットに自動走行を行わせるには、ロボット自身に「今、自分はどこにいるのか」を認識させる必要があります。そのため、あらかじめ環境地図を作成しておくことが重要です。今回、この地図を作成するためにSLAM(Simultaneous Localization and Mapping)という技術を使用します。SLAMは、自己位置推定と地図作成を同時に行う技術のことです。

　SLAMでは、自己位置推定にオドメトリ(Odometry)という手法がよく使用されます。オドメトリとは、ロボットに取り付けられたセンサを使って移動量を計算し、その値を積算していくことで現在の位置を求める方法です。例えば、ホイールオドメトリでは、ロボットの車輪の回転量をエンコーダで読み取り、そのデータからロボットがどれだけ移動したかを計算します。そして、その移動量を積算して現在の位置を求めます。

ホイールオドメトリ

　次に、地図作成について説明します。地図作成では、ロボットに取り付けられたカメラやLiDARなどのセンサから得られるデータを使用します。まず、これらのセンサを使用して、ロボットの周囲の環境情報を取得します。取得したデータから、環境の特徴的な点(特徴点)を抽出します。これを別の地点でも行い、各地点での特徴点をスキャンマッチング(Scan Matching)という手法で照合し、繋ぎ合わせることで地図を更新していきます。

スキャンマッチング

　さらに、ループ閉じ込み(Loop Closure)という手法を使用して地図の整合性を向上させることもあります。これは、ロボットが以前通った地点に戻った際、過去のデータと現在のデータを照合し、誤差を修正する手法です。
　このようにして、1つの大きな地図を作成していきます。

　ROSにおけるSLAMでは、主に3つのTFフレームmap、odom、baseが使用されます。TFツリーの構成はmap→odom→baseとなっております。以下に各フレームの説明を記します。

mapフレーム ... SLAM開始時点のロボットの位置、かつ作成する地図の原点。odomフレームとは違い、固定
odomフレーム ... SLAM開始時点のロボットの位置。オドメトリで発生する誤差を吸収するために存在し、時間が経つにつれ徐々にずれていく可能性がある
baseフレーム ... 現在のロボットの位置

slam-toolbox

　ROS 2の標準SLAMパッケージはslam_toolboxです(ROS2 humble時点)。slam_toolboxでは、LiDARとホイールオドメトリを使ってSLAMを行います。

slam-toolboxのインストール

　まずslam-toolboxのインストールを行います。以下のコマンドを実行してください。

sudo apt install ros-humble-slam-toolbox

slam-toolboxの設定ファイルの用意

　slam_toolboxの設定ファイルを用意します。デフォルトの設定ファイルはmapper_params_online_async.yamlです。デフォルトではbase_frameがbase_footprintに設定されているため、以下のyamlファイルを作成し~/ros2_ws/src/sim_py_01/configに配置します。今回、ファイル名はslam_params.yamlとしています。

~/ros2_ws/src/sim_py_01/config/slam_params.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
    use_map_saver: true
    mode: mapping #localization

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 5.0
    resolution: 0.05
    min_laser_range: 0.0 #for rastering images
    max_laser_range: 20.0 #for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    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

　以下に設定可能なパラメータの一部を記します。各フレーム名やscan_topicなどは、使用するロボットの設定に合わせてください。

toolbox関連(一部のみ)

パラメータ名	説明	型	単位	デフォルト
odom_frame	odomフレーム名	string	-	odom
map_frame	mapフレーム名	string	-	map
base_frame	baseフレーム名	string	-	base_footprint
scan_topic	scanトピックの絶対パス	string	-	/scan
transform_publish_period	mapフレームからodomフレームへのTFをpublishする時間間隔 ※0を設定するとTFはpublishされない	double	sec	0.05
map_update_interval	地図の更新間隔	double	Hz	10.0
resolution	生成する地図の解像度	double	m/pixel	0.05
min_laser_range	地図の描画に使用する最小のスキャン範囲	double	m	0.0
max_laser_range	地図の描画に使用する最大のスキャン範囲	double	m	25
transform_timeout	TFが利用可能になるまで待機する時間	double	sec	0.5

マッチング関連(一部のみ)

パラメータ名	説明	型	単位	デフォルト
use_scan_matching	オドメトリを調整するためにスキャンマッチングを使用するか	bool	-	true
minimum_travel_distance	スキャン間の最小の移動距離	double	m	0.5
minimum_travel_heading	スキャン間の最小の方向変更	double	rad	0.5
scan_buffer_size	スキャンマッチング用に保存される一連のスキャンの個数	int	-	10
scan_buffer_maximum_scan_distance	スキャンマッチング用に保存される一連のスキャンにおける、最初と最後のスキャン間の最大距離	double	m	10
loop_search_maximum_distance	現在の位置からこの距離以下のスキャンは、ループ閉じ込みの対象となる	double	m	3.0
do_loop_closing	ループ閉じ込みを有効にするか	bool	-	true
loop_match_minimum_chain_size	保存された一連のスキャンの個数がこの値より小さい場合、ループは閉じられない	int	-	10

launchファイルの作成

　以下のファイルを~/ros2_ws/src/sim_py_01/launchに配置します。今回、ファイル名はwheel_robot_simple_slam.launch.pyとしています。

~/ros2_ws/src/sim_py_01/launch/wheel_robot_simple_slam.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    package_dir = get_package_share_directory('sim_py_01')
    urdf = os.path.join(package_dir, 'urdf', 'wheel_robot_simple.urdf')
    rviz = os.path.join(package_dir, 'rviz', 'wheel_robot_simple.rviz')
    world = os.path.join(package_dir, 'world', 'maze.world')
    slam_params = os.path.join(package_dir, 'config', 'slam_params.yaml')
    os.environ['GAZEBO_MODEL_PATH'] = os.path.join(package_dir, 'models')

    slam_package_dir = get_package_share_directory('slam_toolbox')

    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf],),

        Node(
            package='joint_state_publisher',
            executable='joint_state_publisher',
            name='joint_state_publisher',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=[urdf],),

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

        ExecuteProcess(
            cmd=['gazebo', '--verbose', '-s',
                 'libgazebo_ros_factory.so', world],
            output='screen',),

        Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            name='urdf_spawner',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-topic', '/robot_description',
                       '-entity', 'wheel_robot_simple'],),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(slam_package_dir, 'launch',
                             'online_async_launch.py')),
            launch_arguments=[('slam_params_file', slam_params)])
    ])

setup.pyの編集

　新たに作成したparamディレクトリをビルド対象に含めるため、setup.pyを修正します。以下の記述をdata_files[]の中に追加してください。

~/ros2_ws/src/sim_py_01/setup.py(一部のみ)

(os.path.join('share', package_name, 'config'), glob('config/*')),

ビルド

　以下のコマンドを実行してsim_py_01パッケージをビルドします。

cd ~/ros2_ws
colcon build --packages-select sim_py_01
source ~/ros2_ws/install/setup.bash

SLAMの実行

　以下のコマンドを実行すると、GazeboとRvizが立ち上がります。

ros2 launch sim_py_01 wheel_robot_simple_slam.launch.py

　立ち上がったRviz上で以下の2つの項目を設定します。

FixedFrameをbase_linkからmapに変更
Addをクリックし、By topicから/map内のMapと/scan内のLaserScanを追加

/map、/scan追加後のRviz

地図作成

　作成される地図は、上記の画像のように、3色のピクセルで表示されています。各色の意味は以下の通りです。

白色 ... 障害物が存在しないピクセル
黒色 ... 障害物が存在するピクセル
灰色 ... 不明なピクセル

　地図作成では、環境内でロボットを移動させ、灰色のピクセルを白色か黒色に染めていくことが求められます。

　別のターミナルを立ち上げ、以下のコマンドでロボットを動かします。

ros2 run turtlebot3_teleop teleop_keyboard --ros-args --remap cmd_vel:=/wheel_robot_simple/cmd_vel

ロボットを動かせれば良いので、以下のようにteleop_twist_keyboardを起動しキーボード入力で動かしたり、直接/wheel_robot_simple/cmd_velトピックを投げたりしても問題ありません。

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/wheel_robot_simple/cmd_vel

　環境内でロボットを移動させることによって、地図を更新していきます。

地図の更新過程

地図の保存

　今回、作成した地図はmapsディレクトリ下に保存します。そのため、別のターミナルを立ち上げ、以下のコマンドでmapsディレクトリを作成しておきます。

mkdir ~/ros2_ws/src/sim_py_01/maps

十分に完成した地図

　上記の画像のように、Rviz上の地図が十分に完成したら、別のターミナルから以下のコマンドを実行します(map_01の箇所が保存ファイル名となります)。

ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap "name:
  data: '~/ros2_ws/src/sim_py_01/maps/map_01'"

　コマンドを実行すると、mapsディレクトリ下にmap_01.pgmとmap_01.yamlが追加されます。pgmファイルは画像ファイルであり、Rviz上の地図がそのまま保存されます。yamlファイルはテキストファイルであり、地図のメタ情報が保存されます。

~/ros2_ws/src/sim_py_01/maps/map_01.pgm

~/ros2_ws/src/sim_py_01/maps/map_01.yaml

image: map_01.pgm
mode: trinary
resolution: 0.05
origin: [-2.85, -2.82, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

　yamlファイルに記述されるパラメータを下にまとめます。

パラメータ名	説明
image	pgmファイル等の画像ファイルのパス。絶対パスでもyamlファイルからの相対パスでも設定可能
mode	各ピクセルの値の解釈。「trinary」「scale」「raw」のいずれかを設定可能。デフォルト値は「trinary」。各値の詳細は下記を参照
resolution	地図の解像度。単位はm/pixel
origin	地図の左下のピクセルの2次元座標(x, y, yaw)
negate	地図の白色と黒色の意味を逆転させるか(下2つのパラメータは影響を受けない)。詳細は下記を参照
occupied_thresh	この値よりも大きい値のピクセルは障害物が存在すると判断する。詳細は下記を参照
free_thresh	この値よりも小さい値のピクセルは障害物が存在しないと判断する。詳細は下記を参照

各ピクセルの値の解釈について

　グレースケールの階調$x\in[0, 256)$を持つピクセルについて、ROSメッセージに入れる際、その値がどう解釈されるのかを説明します。
　最初に、negateの値に応じて、整数$x$は浮動小数点数$p$に変換されます。

p=
\begin{cases}
\frac{255-x}{255.0} & \text{if $negate=false$,} & \text{※黒色(0)が最高値(1.0)、白色(255)が最低値(0.0)} \\
\frac{x}{255.0} & \text{if $negate=true$,} & \text{※白色(255)が最高値(1.0)、黒色(0)が最低値(0.0)}
\end{cases}

　そして、modeの設定値に応じて、そのピクセルの解釈がなされます。

trinary

　出力は$0$、$100$、$-1$のいずれかになります。

\begin{cases}
100 & \text{if $p>occupied\_thresh$,} & \text{※障害物が存在するピクセルとなる} \\
0 & \text{if $p<free\_thresh$,} & \text{※障害物が存在しないピクセルとなる} \\
-1 & \text{otherwise,} & \text{※不明なピクセルとなる}
\end{cases}

scale

　出力は範囲$[0,100]$の値もしくは$-1$になります。$-1$を出力するには、pngファイルのアルファチャンネルを使用します。この場合、透明度は不明と解釈されます。

\begin{cases}
100 & \text{if $p>occupied\_thresh$,} \\
0 & \text{if $p<free\_thresh$,} \\
99\times\frac{p-free\_thresh}{occupied\_thresh-free\_thresh} & \text{otherwise,}
\end{cases}

raw

　階調$x$をそのまま出力します。範囲は$[0,255]$です。

おわりに

　今回は、ROS 2においてslam_toolboxを用いた地図作成を行う方法について紹介しました。次回の記事は、今回作成した地図を基に、Navigation2を使用してロボットに自動走行をさせていきます。