ROS 2においてslam_toolboxを用いた地図作成を行う方法

# はじめに
　今回から2つの記事にまたがって、ROS 2において、対向二輪型ロボットに自動走行を行わせる方法について記載します。今回の記事では、自動走行に必要な環境地図の作成を、slam_toolboxを用いて進めていきます。
　使用するロボットや環境については、前回作成したものを一部流用しています。
 
::: note
この記事は[ROSに関する投稿](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d)の一部です。
目次は[こちら](https://qiita.com/kccs_yoshiteru-imamura/items/ef418e9beb508ac52c5d#%E7%9B%AE%E6%AC%A1)
:::

 ### 前提条件
 　今回の記事は以下の環境で動かすこと前提に記載しています。
||条件|
|---|---|
|OS|Ubuntu 22.04|
|ROS|ROS 2 humble|

# 準備
　まずは、[前回の記事](https://qiita.com/kccs_izumi-egashira/items/5195581f3a45ac6800ea)で作成した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](https://github.com/gazebosim/gazebo-classic/blob/d0c34b8a5d6bc9a3271000fb1baeb3f8d9f43afa/media/materials/scripts/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倍したものを対角線とするひし形が、四角錐の底面となります。

|摩擦円錐|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/e50fd105-b1e1-c2b4-d91d-3d6ec4c9525b.png">|

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

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

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

|設定を追加する前での前進|設定を追加した後での前進|
|:-:|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/f766a592-d21b-f77e-9d7c-fc3f6843b777.gif">|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/fd73f91b-f8c7-a745-a16f-705f56936044.gif">|

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

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

|ホイールオドメトリ|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/c791c661-9756-07c7-72a9-c2bce098ad9b.png">|

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

|スキャンマッチング|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/a55a3e18-2249-2784-a9f0-892a62924aac.png">|

　さらに、ループ閉じ込み(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](https://github.com/SteveMacenski/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](https://github.com/SteveMacenski/slam_toolbox/blob/ros2/config/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する時間間隔<br>※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つの項目を設定します。

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

|/map、/scan追加後のRviz|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/a0a432ad-0012-d8f2-a7b0-7190fae838b8.png">|

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

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

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

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

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

::: note
ロボットを動かせれば良いので、以下のように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
```
:::

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

|地図の更新過程|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/524fb09e-5a7d-3ea4-6464-8afda97c2343.gif">|

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

```
mkdir ~/ros2_ws/src/sim_py_01/maps
```

|十分に完成した地図|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/e952a831-85cb-f60f-6276-68a829b21cbc.png">|

　上記の画像のように、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|
|:-:|
|<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3958579/fe64532a-7098-7af9-4d9f-609733283f33.png">|

```~/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$に変換されます。

```math
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$のいずれかになります。

```math
\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ファイルのアルファチャンネルを使用します。この場合、透明度は不明と解釈されます。

```math
\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を使用してロボットに自動走行をさせていきます。