WSL2の環境下において、Traffic-Editorを使って複数ロボットとエレベータの連携をシミュレーションできる環境を構築してみた

# 1. はじめに
[先日の投稿](https://qiita.com/bebebefu/items/c787bfa2d9daf04aab4c)では、WSL2の環境下において、traffic-Editorを使って複数ロボットの移動をシミュレーションできる環境を構築し、複数台のロボットを設定した経路に沿って動かすことができました。
今回はtraffic-editorを使ってエレベータ付きの2階建ての建物環境を構築して複数台のロボットを動かすことについて、取り組んでみたいと思います。

# 2. 実行環境
- CPU: CORE i7 7th Gen
- メモリ: 32GB
- GPU: GeForce RTX 2070
- OSと構築した環境: Window11でWSL2にUbuntu22.04をインストールした環境
- ROS2: Humble
- Open-RMFの構築環境：[先日の投稿](https://qiita.com/bebebefu/items/c787bfa2d9daf04aab4c)で構築した環境では、ロボットがなぜかエレベータに乗車できなかったため、[[1]](#参考記事やサイト)や[[2]](#参考記事やサイト)（Building from sourceを実行）の手順で環境を構築しました。

# 3. 方針
- [先日の投稿](https://qiita.com/bebebefu/items/c787bfa2d9daf04aab4c)の記事で作成したEnergyPlusのExercise 2の建物モデルを1階として、同じ大きさのフロアを2階として作成し、建物モデルに追加します。
- 2階のレイアウトは1階と同じにしてしまうと紛らわしいので少し変えます。
- エレベータを1台だけ建物モデルに追加して、ロボットがそれを使って階をまたいだ移動ができるように設定します。

# 4. 手順
建物モデルの作り方については、[先日の投稿](https://qiita.com/bebebefu/items/c787bfa2d9daf04aab4c)に詳細を記載しましたので、ここでは「どうやって2階部分を追加したか」や「どうやってエレベータを環境に追加したか」の部分を中心に記載していきたいと思います。

## 4.1 Traffic editorの起動
ターミナルを開き以下のコマンドを実行して起動します。
```:terminal
cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
traffic-editor
```
## 4.2 2階部分の追加
traffic-editorの画面で、以下の図で赤枠で囲んだ「levels」のタブを選択後に「Add」をクリックするとダイアログボックスが出ますので、nameを「L2」にし、参考図面としては1階と同じpngファイルを読み込みました。
![L2の追加.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/d455a31a-6e25-5bf3-f4c4-c161682c66a8.png)

後述するエレベータや参照点が設定された後の画像になってしまっていますが、2階部分の構成は以下の図に示します。レイアウトとしては2つの部屋があり、エレベータから降りたらそれぞれの部屋につながる2つの経路を設定しています。
ここで忘れがちなのが「levels」のタブで設定できるフロアの高さ（Zの値）です。ここを0にしてしまうと1階と同じ高さに2階ができてしまいますので、私はここを5.0に設定しました。
![2階部分.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/8257d779-d307-393b-b061-a41889a1c8c8.png)

## 4.3 エレベータの追加
エレベータは以下の図で赤枠で囲んだ「lifts」のタブを選択後に「Add」をクリックするとダイアログボックスが出ますので、その中で各種設定をしていきます。

以下の図でそれぞれの項目を色分けしましたので、簡単に解説します。
- 緑枠：エレベータの名前を設定
- 黄枠：エレベータが経由する階や初期階などを設定
- 青枠：エレベータの設置位置を設定（私は数字を直接打って位置を調整しました）
- オレンジ枠：エレベータの大きさを設定
- 黄緑枠：Pluginの有効無効の設定（これをYesにしないとロボットとの連携ができませんでした）
- 紫枠：エレベータ内へのwaypoint(ロボットが通る点)の設定（エレベータの中央に1点waypointが追加されます）
- 灰色枠：エレベータのドアの位置や大きさの設定（設定するとダイアログボックス上にドアの位置や大きさが表示されます。）

![エレベータの追加.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/8883f3c4-65be-4cdd-20db-fa85e9f9ea8f.png)

これらの設定をして追加されたエレベータは以下の図で赤枠で囲んだところになります。

- 1階部分のエレベータの位置
![1階のエレベータ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/e331bbf2-3a7f-0608-c8f6-6b02ffd583f2.png)
- 2階部分のエレベータの位置
![2階部分のエレベータ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/31fbb1e1-7044-293f-11a8-2597deb05d4b.png)

## 4.4 参照点の追加
[[3]](#参考記事やサイト)ではfiducialというそうですが、多層階にする場合には垂直方向の位置合わせのため、この参照点の追加が必須だそうです。
手順としては、以下の図で赤枠で囲んだアイコンをクリックして、
![fiducials.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/6f3ef1ea-c973-5499-cb6c-516e4d2dc8e6.png)

次に参照点を設定したいところをクリックして、以下の図のように"ref1"などと名前を設定します。そして他階にも位置合わせしたい箇所に同じ名前の参照点を設定します。私はとりあえず壁の頂点に3つほど参照点を設定しました。
![スクリーンショット 2024-03-16 074700.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3581920/5eb325b7-7ccf-d097-abfa-13a8fd7ee85b.png)

## 4.5 ロボットシミュレーションを行うための設定
### launchファイルの作成
"~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/launch"の中に以下の内容のevtest.launch.xmlを作成しました。また本記事では特に使いませんが、dashboard_configのファイルはgithubで提供されているデモのclinicをそのまま利用しました。

```xml:rmf_demos内のevtest.launch.xml
<?xml version='1.0' ?>

<launch>
  <arg name="use_sim_time" default="false"/>

  <!-- Common launch -->
  <include file="$(find-pkg-share rmf_demos)/common.launch.xml">
    <arg name="use_sim_time" value="true"/>
    <arg name="viz_config_file" value ="$(find-pkg-share rmf_demos)/include/evtest/evtest.rviz"/>
    <arg name="config_file" value="$(find-pkg-share rmf_demos_maps)/evtest/evtest.building.yaml"/>
    <arg name="dashboard_config_file" value="$(find-pkg-share rmf_demos_dashboard_resources)/clinic/dashboard_config.json"/>
  </include>

  <!-- TinyRobot fleet adapter -->
  <group>
    <include file="$(find-pkg-share rmf_demos_fleet_adapter)/launch/fleet_adapter.launch.xml">
      <arg name="use_sim_time" value="$(var use_sim_time)"/>
      <arg name="nav_graph_file" value="$(find-pkg-share rmf_demos_maps)/maps/evtest/nav_graphs/0.yaml" />
      <arg name="config_file" value="$(find-pkg-share rmf_demos)/config/evtest/tinyRobot_config.yaml"/>
    </include>
  </group>

</launch>
```

次に"~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_gz_classic/launch"の中に以下の内容のevtest.launch.xmlを作成しました

```xml:rmf_demos_gz_classic内のevtest.launch.xml
<?xml version='1.0' ?>

<launch>
  <arg name="gazebo_version" default="11"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="failover_mode" default="false"/>

  <!-- Common launch -->
  <include file="$(find-pkg-share rmf_demos)/evtest.launch.xml">
    <arg name="use_sim_time" value="true"/>
    <arg name="failover_mode" value="$(var failover_mode)"/>
  </include>

  <!-- Simulation launch -->
  <include file="$(find-pkg-share rmf_demos_gz_classic)/simulation.launch.xml">
    <arg name="map_name" value="evtest" />
    <arg name="gazebo_version" value="$(var gazebo_version)" />
  </include>

</launch>
```
### configファイルの作成
以下の手順を実施しました。
- "~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/config"の中に"evtest"という名前のフォルダを作成。
- 同じフォルダ内にある"config/clinic"の中にある"tinyRobot_config.yaml"を"evtest"の中にコピー。

**注**: このyamlファイルではそれぞれのtinyRobotの充電スポットの名前を"tinyRobot_1_charger"、"tinyRobot_2_charger"としていますのでtraffic-editorで設定する充電スポットの名前もそれに合わせる必要があります。

### rvizファイルの作成
以下の手順を実施しました。
- "~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/launch/include"の中に"evtest"という名前のフォルダを作成。
- 同じフォルダ内にある"include/clinic"の中にある"clinic.rviz"を"evtest.rviz"として"evtest"の中にコピー。

## 4.6 ビルド
上記の設定が終わったら以下のコマンドでビルドをします。
```:terminal
cd ~/rmf_ws
source /opt/ros/humble/setup.bash
export CXX=clang++
export CC=clang
colcon build
```
## 4.7 シミュレーション環境の起動と巡回の指示
以下のコマンドでgazeboとrvizを起動します。

```:terminal(1)
cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest.launch.xml
```

そして別ターミナルを開き、以下のようにして2つのタスクを投げました。
```:terminal(2)
cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 run rmf_demos_tasks dispatch_patrol -p tinyRobot_1_charger point_0_L2 -n 3 --use_sim_time
ros2 run rmf_demos_tasks dispatch_patrol -p tinyRobot_2_charger point_1_L2 -n 3 --use_sim_time
```

# 5. 結果
上記のコマンドで起動・タスクを出してロボットを動かしたときの動画が以下です。動画は3倍速で流しています。また左側の画像がrivz側（左上が2階、左下が1階）の様子、右側の画像がgazebo側（右上が2階、右下が1階）の様子です。

**注**：同時録画ではなく画面毎にシミュレーションの様子を別撮りしました。そのためスタートは合わせていますが、徐々に動きがずれていきます。なお、ロボットにやらせていることは同じです。

<iframe width="560" height="315" src="https://www.youtube.com/embed/vMSw7vwA5EY?si=FcI0fsrhNP4MBFfe" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>
traffic-editorで設定した経路をたどりながら2台のtinyRobotがエレベータと連携しながら階をまたいで動いている様子が見られます。

# 6. まとめ
上記の手順にて、WSL2の環境下において、今回はtraffic-editorを使ってエレベータ付きの2階建ての建物環境を構築して複数台のロボットを動かすことについて成功したと考えられます。今後は、もうちょっと複雑なタスクの指示、ブラウザからの指示、配送シミュレーション、他のソフトウェアとの連携などを試してみたいと考えています。

# 参考記事やサイト
[1][open-rmf/rmf_demos](https://github.com/open-rmf/rmf_demos)
[2][open-rmf/rmf](https://github.com/open-rmf/rmf)
[3][Traffic Editor](https://osrf.github.io/ros2multirobotbook/traffic-editor.html)

# 関連記事
- [WSL2環境下の複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる　軌跡プロット編](https://qiita.com/bebebefu/items/4f525708768c5be74bfd)にて、本記事で構築した環境で動いているロボットの軌跡を可視化してみました。

- [WSL2環境下で構築したOpen-RMFで複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる　タスクステータス取得編](https://qiita.com/bebebefu/items/2adbc9304f5d3024085d)にて、本記事で構築した環境で動いているロボットのタスクステータスを取得してみました。

- [WSL2環境下で構築したOpen-RMFで複数ロボットとエレベータの連携をシミュレーションできる環境で遊んでみる　人間とtemi出現編](https://qiita.com/bebebefu/items/d4baaf73f8a2c2f9e4ed)にて、本記事で構築した環境で動いているロボットの見た目を人間とtemiにしてみました。