ROS2 Navigation2 ~slam-toolboxでSLAMする~

#はじめに

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

#slam_toolboxについて

https://github.com/SteveMacenski/slam_toolbox

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

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

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

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

![rviz_plugin.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/e4f66538-95aa-c175-3d24-fd9f76b2556a.png)


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

https://roscon.ros.org/2019/talks/roscon2019_slamtoolbox.pdf

#実行環境

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


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

#Install

### slam_toolboxのインストール

以下を実行

```bash
$ sudo apt install ros-foxy-slam-toolbox
```

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

```bash
$ 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を動かすまで](https://qiita.com/porizou1/items/d63a41fc1e478dfa5ab6)


# 地図の作成

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

```bash
$ ros2 launch  turtlebot3_gazebo turtlebot3_world.launch.py 
```

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

```bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard 
```

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

```bash
ros2 launch slam_toolbox online_async_launch.py 
```

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

```bash
rviz2
```

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

![Screenshot from 2021-04-19 20-28-31.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/18178fb2-373c-100f-b8ca-b3cb4808b113.png)

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

![rviz_screenshot_2021_04_19-19_46_35.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/15d2be24-35d1-7d1b-ddd4-540b90626550.png)

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


# 地図の保存

![rviz_screenshot_2021_04_19-19_56_15.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/08b8fa19-cc9e-3647-2c0c-ecab6e057a8d.png)


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

以下でmapの保存

```bash
$ ros2 run nav2_map_server map_saver_cli -f ~/map
```


```bash
[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.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/96f71ab1-4ec0-0559-2c9b-cbe90bdba0b8.jpeg)


```yaml: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シミュレーションを起動

```bash
$ ros2 launch  turtlebot3_gazebo turtlebot3_world.launch.py 
```

Navigation2を起動


```bash
$ ros2 launch  turtlebot3_navigation2 navigation2.launch.py
```


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

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

![Videotogif (7).gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/9815e100-d0bb-adf3-9d34-ae8b2ed72c38.gif)


#おわりに

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


#参考

https://rt-net.jp/mobility/archives/14503

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