ロボット群制御：実践編 ～ ROS/ROS 2混在環境でOpen-RMFを使って複数のturtlebot3を動かしてみた

# はじめに
[前回の投稿](https://qiita.com/yoshi-ima/items/1fd7a6c9fee4f0ab3171)で、Free Fleetを使って、複数のTurtelbot3を動かすSimulation環境を構築しました。  
今回は、下の図にあるようにFree Fleet ServerとOpen-RMFを接続して、TerminalからOpen-RMFタスク（以下、タスク）の指示を出してみます。

<kbd>

```plantuml
@startuml
Title 全体構成の概略図
skinparam componentStyle rectangle

package "ROS 2(Foxy)" {
    [Terminal] 
    package "Open-RMF" #Business {
        [RMF] as RMF_CORE  #Business
        [RMF Fleet Adapter] as RMF_FLEET #Business

        RMF_CORE - RMF_FLEET
    } 

    package "Free Fleet Server" {
        [Turtelbot3 Server] as FF_SERV
    }
    RMF_FLEET -- FF_SERV
}

package "ROS(Noetic)" {
    package "Free Fleet Client" {
        [Trutelbot3 Client 3] as TB2
        [Trutelbot3 Client 2] as TB1
        [Trutelbot3 Client 1] as TB0
    }

    package "Turtelbot3 Simulator" {
        [tb3_0] as tb3_0
        [tb3_1] as tb3_1
        [tb3_2] as tb3_2
        [Gazebo] as GAZEBO

        tb3_0 -- GAZEBO
        tb3_1 -- GAZEBO
        tb3_2 -- GAZEBO
    }

    database "Gazebo Model" {
        [House] as HOUSE
    }
    HOUSE -- GAZEBO

    TB2 -- tb3_2
    TB1 -- tb3_1
    TB0 -- tb3_0
}
FF_SERV -- TB0
FF_SERV -- TB1
FF_SERV -- TB2

Terminal -- RMF_CORE


@enduml
```

</kbd>

# 環境
 ・OS  : Ubuntu20.04  
 ・ROS Version : ROS → Noetic, ROS 2→Foxy

# 環境構築
下記URLにあるOffice Worldを参考に構築しました。  
Gazebo環境は構築してあるので、Traffic Editorを使ったOpen-RMFの地図生成やタスクの実行の部分を参考にしました。  
https://github.com/open-rmf/rmf_demos/tree/foxy#Office-World

## Simulation環境のハマりポイント
今回もハマりポイントはありましたが、サマリーを記載します。

### バッテリー状態の更新
バッテリの状態（％）をOpen-RMFに通知する必要があります。  
前回の環境では、バッテリの状態が通知されませんので、[Gazeboのbattery plugin](https://github.com/nilseuropa/gazebo_ros_battery)をインストールしました。 インストールすると、バッテリの状態が通知されるようになります。  
しかし、Open-RMFの満充電は1.0で、Gazebo Battery Pluginは100とバッテリーの単位が違います。今回は、簡単な動作確認が主なので、ソースコードを修正して強引にOpen-RMFの単位に合わせました。

### ROS 2/ROS間のROS Time同期
「use_sim_time」オプションを使うので、/clockのトピックベースのROS Timeが使われます。  
今回の構成では、/clockはROS側のgazebo_ros packageが出力します。ROS 2側でもROS::Timeを使っているので、/clockをROS 2に伝える必要があります。伝える方法ですが、[ros1_brige](https://github.com/ros2/ros1_bridge)を使いました。

## 経路設定
Open-RMFで使用する経路データを作成する必要があります。  
Turtelbot3の開発環境に付いてくる「house」ワールドベースにTraffic Editorで作成します。  
Traffic Editorで編集した地図は、グラフィック座標系なのでROSの地図座標に変換する必要があります。「rmf_demos_map」パッケージに追加するとcolcon build時にフォルダーを検索して、自動で変換するようになっています。今回は、これを利用します。

### RMF経路データ作成

最初に、SLAMの地図データをpngに変換します。  
私は、pgm→png変換は、Visual Studio Codeのpluginで変換しました。

```sh:SLAM地図データの場所
~/Work/open-rmf/free_fleet/ros1/src/turtlebot3_simulations/turtlebot3_gazebo/maps/house.pgm
```

pgmファイルをpngファイルに変換して、以下のフォルダーに置きます。

```sh:保存フォルダー作成＆pngファイルコピー
mkdir ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house
cp house.png ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house
```
### Traffic Editorによる経路設定
次にTraffic Editorを使ってで経路を設定します。

```sh:Traffic Editor起動手順
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
traffic-editor
```

さきほど、コピーしたpngファイルを読み込ませます。 
使い方は、公式に詳しく記載してありますので、そちらを参考にして下さい。  

https://osrf.github.io/ros2multirobotbook/traffic-editor.html
![house01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/fc71f80a-8114-f780-0441-68bd2a754642.png)


重要なのがLevel Nameで、ここではOpen-RMF Demoに合わせて「L1」にしています。  
作成した地図情報は、以下の場所に保存します。
```sh:地図保存フォルダー
~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos_maps/maps/house
```
保存名を「house」にすると「house.building.yaml」ファイルが出力されます。
また、他のフォルダーのoffice.project.yaml参考にhouse.project.yamlを作成しました。

最後に、colcon buildすると経路設定完了です。  
```sh:build
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
colcon build --symlink-install
```
上手くビルドできない場合は、「colcon build --cmake-clean-cache」してから再度ビルドして下さい。

## 車両スペック
車両スペックは、tinyRobot設定ファイルをベースに変更します。

```sh:車両スペック設定ファイル作成
cd ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos/launch/include/adapters
cp tinyRobot_adapter.launch.xml turtelbot3_adapter.launch.xml
```
スペックは、[Turtelbot3の公式HP](https://emanual.robotis.com/docs/en/platform/turtlebot3/features/#features)を参考に設定しました。

## Fleet Adapterと接続
Fleet Name = "tb3"、Robot Preifx = "tb3_"としています。

Open-RMFとFleet Adapterの起動用launchファイルを準備します。  
下記の様に、office.launch.xmlをhouse.launch.xmlにコピーして、パラメータを変更します。

```sh:Open-RMF起動ファイル作成
cd ~/Work/open-rmf/free_fleet/ros2/src/demonstrations/rmf_demos/rmf_demos/launch
cp office.launch.xml house.launch.xml
```
「freet_name」と「robot_prefix」と「車両スペックファイル」を設定に合わせて変更します。
ファイルを作成しましたので、colcon buildを実行します。

# 実行手順

Free Fleet関連のモジュールを起動します。
4つTerminalを開き、順番に起動していきます。

```sh:terminal(1:ROS turtelbot3 simulation/Free Fleet Client起動)
cd ~/Work/open-rmf/free_fleet/ros1
source install/setup.bash
export TURTLEBOT3_MODEL=burger; roslaunch ff_examples_ros1 multi_turtlebot3_ff.launch
```

```sh:terminal(2:ROS 2 Free Fleet Server起動)
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch ff_examples_ros2 turtlebot3_world_ff_server.launch.xml
```

```sh:terminal(3:ROS 2 ros1_bridge起動)
cd ~/Work/open-rmf/free_fleet/ros1_bridge_ws
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
source install/setup.bash
ros2 run ros1_bridge dynamic_bridge
```

```sh:terminal(4:ROS2 Open-RMF起動)
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch rmf_demos house.launch.xml
```

# タスク実行

Terminalから以下のコマンドを実行すると、ロボットが動き出します。

```sh:terminal(5:ROS2 ros1_bridge起動)
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 run rmf_demos_tasks dispatch_loop -s goal0 -f goal1 --use_sim_time
ros2 run rmf_demos_tasks dispatch_loop -s goal1 -f goal2 --use_sim_time
ros2 run rmf_demos_tasks dispatch_loop -s goal2 -f goal0 --use_sim_time
```
![house02.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/f3c824b4-7518-48b4-283f-095effa9f410.png)

上の右側の図について、ピンク色の印がロボットの現在位置で、黄色がOpen-RMF側で計画している位置になります。
左側の図にある赤色の矢印が、各ロボットがどこをGoalにしているかを示しています。

今回は、簡単な「Loop」タスクを実行しました。
タスクの実行は場所の指定だけで、Open-RMFが移動させるロボットを選択して目的地までアシストします。
経路が重なってもOpen-RMF側でどちらを優先させるかを判断します。 判断のアルゴリズムは、「greedy algorithm」などが使われているようです。

ただし、黄色の印が先行しすぎて、ピンクの印との距離が離れすぎてエラーになることがありました。
Open-RMFの経路計画は、リアル時間で計算されています。ロボット側はSimulation時間で計算されているので、PCが重くなるとSimulation時間が遅くなり、リアル時間が先行してしまうのが原因の１つではないかと推測しています。

# まとめ
SLAMで作った地図をOpen-RMFに適応させてタスクを実行させることができました。

Free FleetのOpen-RMFへの適応のさせ方も理解できました。
次は、複数Fleetを使って種類が違うロボットを混在させて制御してみます。 

環境構築でいろいろハマってしまったのですが、いろいろ対策していく中でOpen-RMFの理解が深まりました。実際に動かして自力で考えることが理解への近道ということを再認識しました。

[応用編](https://qiita.com/yoshi-ima/items/00919c8eff9c24d1b993)へつづく