2次元地図からGazeboのmodelを作ってナビゲーションしてみた

#目次
[1.記事の内容について](#1-記事の内容について)
[2.map2gazeboを使ってみる](#2-map2gazeboを使ってみる)
[3.map2gazeboで生成したmodelを使ってナビゲーションをしてみる](#3-map2gazeboで生成したmodelを使ってナビゲーションをしてみる)
[4.おわりに](#4-おわりに)
[5.おまけ](#5-おまけ)


# 1. 記事の内容について
こんにちは、12月2日を担当させていただく、BEIKEです。

今年から初めてつくばチャレンジに参加しました。
つくばチャレンジの練習走行では、まずrosbagでのデータ取りを行いました。
rosbagを取っておけば現地じゃなくても、そのデータを基にマッピングや自己位置推定のテスト
を行うなどの作業を行うことが出来ます。

しかし、rosbagのデータでは現地のような環境でナビゲーションをテストすることが出来ないので
出来たら良いのになーと考えていました。

そうだ、2次元地図からGazeboのワールドを作れば良いのでは？！
(2次元地図を自己位置推定やナビゲーションに使っていたので)
調べたらありました。

https://github.com/shilohc/map2gazebo

ということで、map2gazeboを使って2次元地図から
Gazeboのmodelを作ってナビゲーションしてみた話をこれからさせて頂きます。

# 2. map2gazeboを使ってみる
### 2.1 ワークスペースの構築からビルドまで
```
mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src
git clone https://github.com/shilohc/map2gazebo.git
cd ~/catkin_ws && catkin build
source ~/catkin_ws/devel/setup.bash
```

### 2.2 map2gazebo.launchの実行
`map2gazebo.launch`を実行します。
引数のparams_fileにはmodelを生成するためのパラメータが書かれたファイルのパスを渡します。
[パラメータはmodelの出力形式(.stl、.dae)やmodelの高さなどがあります。](https://github.com/shilohc/map2gazebo/blob/master/config/defaults.yaml)

```
roslaunch map2gazebo map2gazebo.launch params_file:=$(rospack find map2gazebo)
/config/defaults.yaml
```

次に生成したいmodelの基になるマップのトピックをmap_serverにてpublishします。

```
rosrun map_server map_server map.yaml
```

そうすると、以下のようにmap2gazeboがマップを受け取ったメッセージが出るのでしばらく待ちます。
`[INFO] [1638213724.217835]: Received map`

[![Image from Gyazo](https://i.gyazo.com/c438b0d7b364fa47dbabac0a39ad706d.png)](https://gyazo.com/c438b0d7b364fa47dbabac0a39ad706d)ｌ
map2gazebo.launchやdefaults.yamlに出力先のパスをしていなければデフォルトで`map2gazebo/models/map/meshes`に生成されたmodelが出力されます。

以下のようなメッセージが出れば完了です。
`[INFO] [1638213779.569149]: Exported STL.  You can shut down this node now`
[![Image from Gyazo](https://i.gyazo.com/33886b36bab476fcf21f70a72dda277b.png)](https://gyazo.com/33886b36bab476fcf21f70a72dda277b)

### 2.3 生成されたmodelの確認

今回modelを生成するために使用した地図はこちら(つくば市役所前から信号ありの横断歩道までです)
[![Image from Gyazo](https://i.gyazo.com/fab5792154a03966c5770a7ee9c6e3c6.png)](https://gyazo.com/fab5792154a03966c5770a7ee9c6e3c6)

生成したmodelを確認してみます。
map2gazeboにある`gazebo_world.launch`を使用して確認します。

```
roslaunch map2gazebo gazebo_world.launch
```
占有格子地図の占有された部分のみを押し出したようなmodelが出来ました。

[![Image from Gyazo](https://i.gyazo.com/69206865b5d74b0167b5a9fcc958f635.png)](https://gyazo.com/69206865b5d74b0167b5a9fcc958f635)

[衛星写真と一緒に見ると壮観です。](https://gazebosim.org/tutorials?tut=static_map_plugin&cat=build_world)
良い感じのナビゲーションの簡易テスト環境が出来上がりました！
[![Image from Gyazo](https://i.gyazo.com/54f325ad8f2c6604520f60d98bed4c99.png)](https://gyazo.com/54f325ad8f2c6604520f60d98bed4c99)

# 3. map2gazeboで生成したmodelを使ってナビゲーションをしてみる
### 3.1 TurtleBot3を使って試してみる
まず、TurtleBot3のシミュレータ環境の構築を行います。
先程、map2gazeboで使用した`catkin_ws`を使用します。

```
cd ~/catkin_ws/src
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
catkin build
source ~/catkin_ws/devel/setup.bash
```

ワークスペースはこんな感じになってると思います。

```
hoge@hoge:~/catkin_ws/src$ tree -L 1
.
├── map2gazebo
├── turtlebot3
├── turtlebot3_msgs
└── turtlebot3_simulations
```

map2gazeboで生成したmodelを使用した環境を立ち上げるために
こんな感じのGazebo用のlaunchを書きます。

```
<launch>
  <env name="GAZEBO_MODEL_PATH" value="$(find map2gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>

  <arg name="model" default="burger"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find map2gazebo)/worlds/map.sdf"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro"/>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description"/>
</launch>
```

作成したGazebo用のlaunchを立ち上げます。

```
roslaunch turtlebot3_gazebo hogehoge.launch
```

次にマップは作成済みとしてナビゲーションを立ち上げます。

```
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
```
準備は出来たので、2D Nav Goalを適当な場所に置いてナビゲーションしてみます。

![hogehoge.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/280989/44b030d4-852a-e774-29aa-75c2f319d230.gif)

ナビゲーションできました。

### 3.2 実際に使ってた時の動画とつくばチャレンジの本走行の動画
* 実際にナビゲーションのテストをしている様子(waypointやパスプランの確認をした)
<iframe width="1280" height="320" src="https://www.youtube.com/embed/tRTD174K4bE" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

* 実際のつくばチャレンジでの走行（テストした通りのwaypoint通過、パスプランだった（多分））
<iframe width="1280" height="320" src="https://www.youtube.com/embed/TeF1nFxjLRQ" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

上の2つの動画のナビゲーションは[raspicat_navigation](https://github.com/uhobeike/raspicat_navigation.git)というパッケージを使用しました。

https://github.com/uhobeike/raspicat_navigation.git

# 4. おわりに
今回は2次元地図からGazeboのmodelを作ってナビゲーションしてみました。

作成した環境は地面の凹凸は無いものの観測状況は現地の環境に少し近い感じで良いテスト環境が出来上がりました。
[地面の凹凸なども再現したい場合は3次元データからstlなどに変換するとできるのかもしれません。](https://ignitionrobotics.org/api/gazebo/4.0/pointcloud.html)

これで現地の環境に少し近いナビゲーションのテストが出来て良いのではないでしょうか？！

最後まで読んでくださって、ありがとうございました。

# 5. おまけ
この記事の内容に関連した情報を最近知ったので追記します。(2021/12/06)

この記事のやってることの逆で、Gazeboのワールドから2次元マップを作成する方法があるようです。

https://uenota.github.io/dronedoc/ja/build_map_gazebo_plugin/build_map_gazebo_plugin.html