はじめに

ROS2標準のSLAMパッケージであるslam-toolboxを使ってGazebo上でSLAMをする手順についてまとめました。
動作確認にはTurtlebot3シミュレーションパッケージを使います。

slam_toolboxについて

ROS2 DasingまではROS2標準のSLAMパッケージがcartographerでしたが、Eloquent以降slam_toolboxという新しいSLAMパッケージに変更されました。

slam_toolboxには以下のような特徴があります。

・Kartoアルゴリズムがベース
・ceres-solverによるICPスキャンマッチングとGraph-Based-SLAM
・オドメトリ必須
・手動で地図調整できるGUIツールがある
・SLAMデータを保存してLocalizationモードで使用できる

以下のようなrviz2プラグインを使って手動でループ閉じ込みできる点などが主な特徴です。

詳しくは以下のroscon2019の資料を参照してください

実行環境

この記事は以下の環境で動かしています。

項目	値
CPU	Ryzen5
OS	Ubuntu20.04.02LTS
ROS2	Foxy Fitzroy

Install

slam_toolboxのインストール

以下を実行

$ sudo apt install ros-foxy-slam-toolbox

以下ソースからビルドする場合(推奨)

$ cd ~/ros2_ws/src
$ git clone https://github.com/SteveMacenski/slam_toolbox.git
$ cd slam_toolbox/
$ git checkout foxy-devel
$ cd ~/ros2_ws
$ colcon build

Navigation2とTurtlebot3シミュレーションパッケージのインストール

Navigation2とTurtlebot3のGazeboシミュレーション環境は以下の記事にしたがってインストール

ROS2 Foxy環境にTurtleBot3のGazeboシミュレーションをインストールしてNavigation2を動かすまで

地図の作成

Turtlebot3 Simulationパッケージの実行
とりあえず環境はTurtlebot3 worldを使用

$ ros2 launch  turtlebot3_gazebo turtlebot3_world.launch.py

Teleop用に/cmd_velをPublishするノードを起動
ここはGazebo上のTurtlebot3を操作できればteleop_twist_joyでも何でもいいです。

ros2 run teleop_twist_keyboard teleop_twist_keyboard

slam_tool_boxノードを起動
async_slam_toolbox_nodeとsync_slam_toolbox_nodeの2種類があるがasyncの方を使用（違いは調査中）
onlineで実行するときはasyncを使えばいいらしい

ros2 launch slam_toolbox online_async_launch.py

作成中の/mapトピックを見るためにrviz2を起動

rviz2

左下のAddからMapを選択してOKをクリック

Topicからトピック名/mapを入力
以下のように現在の/mapが表示される

/cmd_velを送ってTurtlebot3を動かすとrviz2画面のmapが更新される
（/mapトピックの更新がやたら遅いのは調査中）

地図の保存

ある程度全体のmapが出来上がったらNavigation2のnav2_map_serverパッケージのmap_saverを使用して作成した地図を保存します。

以下でmapの保存

$ ros2 run nav2_map_server map_saver_cli -f ~/map

[INFO] [1618829845.625503871] [map_saver]: 
    map_saver lifecycle node launched. 
    Waiting on external lifecycle transitions to activate
    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1618829845.625745924] [map_saver]: Creating
[INFO] [1618829845.626007161] [map_saver]: Saving map from 'map' topic to '/home/porizou/map' file
[WARN] [1618829845.626031354] [map_saver]: Free threshold unspecified. Setting it to default value: 0.250000
[WARN] [1618829845.626052861] [map_saver]: Occupied threshold unspecified. Setting it to default value: 0.650000
[WARN] [map_io]: Image format unspecified. Setting it to: pgm
[INFO] [map_io]: Received a 112 X 103 map @ 0.05 m/pix
[INFO] [map_io]: Writing map occupancy data to /home/porizou/map.pgm
[INFO] [map_io]: Writing map metadata to /home/porizou/map.yaml
[INFO] [map_io]: Map saved
[INFO] [1618829847.259541041] [map_saver]: Map saved successfully
[INFO] [1618829847.259661983] [map_saver]: Destroying

/homeディレクトリ以下にmap.pgmとmap.yamlが生成される。

map.pgm

map.yaml

image: map.pgm
mode: trinary
resolution: 0.05
origin: [-2.96, -2.56, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

yamlファイルの内容はROS1と同じ。
上からimage:イメージファイル、mode:モード、resolution:解像度、origin:原点座標、negate:白黒反転させるかどうか、occupied_thresh:これより大きい場所が障害物、free_thresh:これより小さい場所がフリースペース。

作成した地図を使ってNavigationをする

次は作成した地図を使ってNavigation2でNavigationをします。

~/turtlebot3_ws/src/turtlebot3/turtlebot3/turtlebot3_navigation2/mapディレクトリ以下に先程作成したmap.pgmとmap.yamlをコピー

Turtlebot3シミュレーションを起動

$ ros2 launch  turtlebot3_gazebo turtlebot3_world.launch.py

Navigation2を起動

$ ros2 launch  turtlebot3_navigation2 navigation2.launch.py

rviz2ウィンドウ上の2D Pose Estimateをクリック、map上の現在いる位置をクリックしてロボットの初期位置を設定

Navigation2 Goalをクリック
移動させたい位置と姿勢をrviz2画面をクリックしてゴールを指定
Gazebo上のTurtlebot3が動いてゴールに到着すれば成功

おわりに

次はslam_toolboxをlocalizationモードで動かしてNavigation2のmap_serverとamclの代わりに使用してNavigationを行います。

参考

最新のROS実装のあるLidarSLAMオープンソースソフトウェアまとめ
https://qiita.com/rsasaki0109/items/493c1059ffe3178166bc

ROS2 Navigation2 ~slam-toolboxでSLAMする~