ロボット群制御：応用編 ～ Open-RMFを使って種類の違うロボットを動かしてみた

# はじめに
[前回の投稿](https://qiita.com/yoshi-ima/items/153fd05ef40955d03503)で、Free FleetとOpen-RMFを使ってLoopタスクを実行しました。  
今回は、下の図にあるように種類の違うFleetを接続してブラウザからOpen-RMFタスクの指示を出してみます。   

追加するロボットは、手持ちで簡単に構築できるロボット（以下、AMR）を選びました。 AMRはMelodicで動作するため、Dockerを使った環境を追加します（青色の箇所が前回との差分になります）。

<kbd>

```plantuml
@startuml
Title 全体構成の概略図
skinparam componentStyle rectangle
skinparam actorStyle awesome
skinparam Package {
    BorderColor<<ROS(Melodic)>> Blue
}

package "PC (Ubuntu 20.04)" {

package "ROS 2(Foxy)" {
    package "Open-RMF" #Business {
        [WEB] as RMF_WEB #SkyBlue
        [RMF] as RMF_CORE  #Business
        [Fleet Adapter Turtelbot3] as TB_FLEET #Business
        [Fleet Adapter AMR] as AT_FLEET #SkyBlue

        RMF_WEB - RMF_CORE
        RMF_CORE -- TB_FLEET
        RMF_CORE -- AT_FLEET
    } 

    package "Free Fleet Server" {
        [AMR Server] as AT_SERV #SkyBlue
        [Turtelbot3 Server] as TB_SERV
    }
    TB_FLEET -- TB_SERV
    AT_FLEET -- AT_SERV
}

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

package "Ubuntu 18.04 on Docker" as (USE) << ROS(Melodic) >> {
    package "Turtelbot3 Simulator" {
        [amr_1] as at_0 #SkyBlue
        [tb3_0] as tb3_0
        [tb3_1] as tb3_1
        [tb3_2] as tb3_2
        [Gazebo] as GAZEBO

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

    database "Gazebo Model" {
        [House] as HOUSE
    }
    HOUSE -- GAZEBO
    AT0 -- at_0
    TB2 -- tb3_2
    TB1 -- tb3_1
    TB0 -- tb3_0
}
TB_SERV -- TB0
TB_SERV -- TB1
TB_SERV -- TB2
AT_SERV -- AT0

[ブラウザ] #Skyblue
}
actor User 
User -- ブラウザ
ブラウザ -- RMF_WEB


@enduml
```

</kbd>

動作確認（できるかぎり作らない）が目的なので、このような構成になっています。

# 環境
 ・OS  : Ubuntu20.04  
 ・ROS Version : ROS → Noetc / Melodic, ROS 2 → Foxy

# 環境構築
環境構築のポイントは以下の４つです。

1. MelodicとNoeticの通信
2. 複数ロボットのGazebo環境構築(Melodic側)
3. Free Fleet Clientの複数対応(Noetic側)
4. Web設定

Docker環境については本題ではないので、後述する「付録」に載せておきます。

## MelodicとNoeticの通信
Melodicのroscoreを指定すれば良いので、Noetic側の環境変数を以下のように設定します。  
IPアドレスですが、'ifconfig'して、'docker0'のinetの値を入れました。

```sh:Noetic側のbash
export ROS_MASTER_URL=http://172.17.0.1:11311
```
Melodic側のROSプログラム(roscore)を最初に起動して、上記の環境変数が設定された状態でNoeticのROSプログラムを実行するだけです。

### ハマりポイント
MelodicからNoeticの通信はできているのですが、Open-RMFのBattery値が更新されません。  
Logを見るとMelodicからのBatteryトピック受信でハッシュ値が違うとROSトピック通信のログメッセージが出ています。
同じ構造体名であり、送受信でデータ構造が違うときに、このメッセージが出ます。

ROSの共通メッセージは、変わらないものだと思い込んでいましたが、
調べるとやっぱり、NoeticのBattery構造体に「temperature」メンバーが追加され、Melodicと構造が違います。

https://github.com/ros/common_msgs/commit/4804aa31c4df20c2143c7a09627a09039dafc1e0

対策としてはデータ構造を合わせます。具体的には、[sendor_msgsパッケージ](https://github.com/ros/common_msgs/tree/noetic-devel/sensor_msgs)のnoetic-develブランチをMelodic側のcatkin_ws/srcに取得してcatkin_makeしました。

## 複数ロボットのGazebo環境構築(Melodic側)
MelodicのTurtelbot3は、何も修正することなく動きます。

launchファイルは、Free Fleetサンプルにある[multi_turtelbot3_ff.launch](https://github.com/open-rmf/free_fleet/blob/main/ff_examples_ros1/launch/multi_turtlebot3_ff.launch)を修正したものを使いました。修正といっても、上記URLファイルの63行目以降をコメントアウト（削除）するだけです。  
また、AMRのGazeboモデル・Spawn情報を上記ファイルに追加しました（詳細は省略）。

## Free Fleet Clientの複数対応(Noetic側)
Free Fleet Server/ClientをTurtelbot3用とAMR用のそれぞれ用意します。
Turtelbot3の設定はそのまま使うので、AMRの情報を追加します。
ポイントとしては、AMRとOpen-RMFのDDS DomainをTurtelbot3と重ならない値（43）にしました。

launchファイルは、Free Fleetのサンプルにある[multi_turtelbot3_ff.launch](https://github.com/open-rmf/free_fleet/blob/main/ff_examples_ros1/launch/multi_turtlebot3_ff.launch)を修正したものを使いました。
今度は、Melodic側のlaunchファイルとは逆で、62行目までをコメントアウト（削除）したものを使います。

追加するAMRも上記を参考にしてmulti_turtelbot3_ff.launchに追加します。

```xml:AMR Client情報
  <arg name="atmobi_0_prefix" value="atmobi_0"/>

  <node name="$(arg atmobi_0_prefix)_free_fleet_client_node" 
      pkg="free_fleet_client_ros1"
      type="free_fleet_client_ros1" output="screen">
    <param name="fleet_name" type="string" value="atmobi"/>
    <param name="robot_name" type="string" value="$(arg atmobi_0_prefix)"/>
    <param name="level_name" type="string" value="L1"/>
    <param name="dds_domain" type="int" value="43"/>
    <param name="robot_model" type="string" value="whill"/>
    <param name="max_dist_to_first_waypoint" type="double" value="10.0"/>
    <param name="battery_state_topic" value="/battery_state"/>
    <param name="robot_frame" value="base_link"/>
    <param name="move_base_server_name" value="/atmobi_pkg/move_base"/>
  </node>
```

## Free Fleet Serverの複数対応(Foxy側)
Server側の設定を前述のClientの情報に合わせてAMR情報を追加します。  
https://github.com/open-rmf/free_fleet/blob/main/ff_examples_ros2/launch/turtlebot3_world_ff_server.launch.xml

```xml:AMR用追加データ
  <node pkg="free_fleet_server_ros2"
      exec="free_fleet_server_ros2"
      name="atmobi_fleet_server_node"
      output="both">

    <param name="fleet_name" value="atmobi"/>

    <param name="fleet_state_topic" value="fleet_states"/>
    <param name="mode_request_topic" value="robot_mode_requests"/>
    <param name="path_request_topic" value="robot_path_requests"/>
    <param name="destination_request_topic" value="robot_destination_requests"/>

    <param name="dds_domain" value="43"/>
    <param name="dds_robot_state_topic" value="robot_state"/>
    <param name="dds_mode_request_topic" value="mode_request"/>
    <param name="dds_path_request_topic" value="path_request"/>
    <param name="dds_destination_request_topic" value="destination_request"/>

    <param name="update_state_frequency" value="20.0"/>
    <param name="publish_state_frequency" value="2.0"/>

    <param name="translation_x" value="-10.0"/>
    <param name="translation_y" value="9.1"/>
    <param name="rotation" value="0.0"/>
    <param name="scale" value="1.0"/>

  </node>
```

## Web設定
WebアプリはOpen-RMF Demoパッケージのものを使います。
タスクの開始・終了地点は、Traffic Editorで作ったものに変更します。
そこで、下記URLのJsonファイルを参考に、"Loop"→"places"の値を変更してJsonファイルを作ります。
"Delivery"タスクは、使わないので削除しました。

https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_dashboard_resources/office/dashboard_config.json

作成したJsonファイルはOpen-RMFのlaunchファイルで指定します。  
具体的には、以下のファイルの11行目です。  
https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos/launch/office.launch.xml

実際に起動するファイルは、前回作成したhouse.launchです。([前回の投稿参照](https://qiita.com/yoshi-ima/items/153fd05ef40955d03503#fleet-adapter%E3%81%A8%E6%8E%A5%E7%B6%9A))

# 実行

以下を順番に実行します。

```sh:(1) Simulation実行 [Melodic on Docker]
cd /root/catkin_ws
source ./devel/setup.bash
roslaunch ff_example_ros1 multi_turtelbot3_ff.launch
```

```sh:(2) Fleet Client実行 [Noetic]
export ROS_MASTER_URI=http://172.17.0.1:11311
cd ~/Work/open-rmf/free_fleet/ros1
source install/setup.bash
roslaunch ff_examples_ros1 multi_turtelbot3_ff.launch
```

```sh:(3) Fleet Server実行 [Foxy]
export ROS_MASTER_URI=http://172.17.0.1:11311
cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch my_sample turtelbot3_world_ff_server.launch.xml
```

```sh:(4) ros1_brige実行 [Foxy]
export ROS_MASTER_URI=http://172.17.0.1:11311
source /opt/ros/noetic/setup.bash
source /opt/ros/foxy/setup.bash
cd ~/Work/open-rmf/free_fleet/ros1_bridge_ws
source install/local_setup.bash
ros2 run ros1_bridge dynamic_bridge
```

```sh:(5) RMF実行 [Foxy]
export ROS_MASTER_URI=http://172.17.0.1:11311
source /opt/ros/foxy/setup.bash

cd ~/Work/open-rmf/free_fleet/ros2
source install/setup.bash
ros2 launch rmf_demos house.launch.xml
```
ここまで実行すると下の図のようになります。  
左側から「Open-RMFの経路情報」・「ロボット情報(Rviz)」・「Gazebo画面」です。  
追加されたAMRも画面に出ています。

![起動画面](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/1159b626-b772-bf77-3124-d44ba73cc87f.png)

```sh:(6) ブラウザ実行 [PC]
google-chrome https://open-rmf.github.io/rmf-panel-js/
```
上記のコマンドでブラウザを起動するとRMF Panelのページが表示されます。（ブラウザでURLを指定しても構いません。） 
下の図にある「REFRESH」ボタンを押下すると、登録されているロボットが表示されます。
バッテリーは、強制的に100％にしているので満充電です。([前回の投稿参照](https://qiita.com/yoshi-ima/items/153fd05ef40955d03503#%E3%83%90%E3%83%83%E3%83%86%E3%83%AA%E3%83%BC%E7%8A%B6%E6%85%8B%E3%81%AE%E6%9B%B4%E6%96%B0))

![ロボット状態](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/ea37f59a-231e-e2d8-ba0b-509d14c53579.png)

ブラウザからタスクを実行します。  
Dashbordを変更しているのでWayPointが設定したものになっています。

![タスク選択](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/df5b8cf4-7173-c75d-36c4-b3c2b3dcc45a.png)

開始・終了地点をプルダウンメニューから選択して、「SUBMIT REQUEST」ボタンを押下するとタスクをOpen-RMFにリクエストします。

Tasksの「REFRESH」ボタンを押下するとタスク状態が表示されます。
しかし、下の図にあるようにタスク実行に失敗する時があり、今後解析していきます。

![タスク実行](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/8fb585e4-3deb-77f7-1cf6-b3b6a793b85a.png)

実行途中の様子です。
Worldに対して追加したAMRが大きかったのですが、なんとか通れました。

![実行の様子](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3113036/9896fb14-3eae-dc76-8547-fbf66710ae6c.png)


# まとめ
ブラウザから複数のFleetを使ってタスク実行できることを確認しました。
ここからが調査の本番ですね。

Fleetの外部APIはトピックベースなので、ソース修正することなく簡単にロボットを追加できました。  
ロボット側もmove_baseのAPI体系をサポートしていたら簡単に追加できます。  

ROSを使うと混在環境でも、簡単に動かせるのが良いですね。

# 付録
Docker関連ファイルです。(Proxy環境になります。)
## Melodic on Docker
Melodicは、ROSの[Offical Image](https://hub.docker.com/_/ros)を使いました。  
Dockerを使うときは、**Dockerfile、docker-compose.yaml、起動スクリプト**の３セットを作ります。  

そして、起動スクリプト(以下、**run.sh**)と同じフォルダーにあるcatkin_wsをDockerにマウントさせます。作った後は**run.sh**を実行するだけにしています。
  
編集はホストマシンで行い、ビルド・実行は**run.sh**してDockerコンテナのbashで実行します。

Dockerコンテナの最初の起動は、catkin_wsフォルダーに移動して、以下のコマンドで不足パッケージをインストールすると思います。
```sh:不足パッケージインストール
rosdep install --from-paths src --ignore-src --rosdistro melodic -y
```
しかし、Dockerのコンテナが消えると、もう一度パッケージをインストールする必要があります。  
Localネットワーク環境でコンテナが消えてしまうと不足パッケージをインストールできないので困ります。  
そこで、先に不足パッケージを調べてDockerのImageに入れてます。
こうすることで、コンテナが消えてもパッケージをインストールする必要はありません。

調べ方ですが、最初にシンプルなDocker Imageを作成して、Dockerのコンテナのbashからcatkin_wsフォルダーに移動して、
以下を実行すると不足パッケージが表示されます。

```sh:不足パッケージ調査
rosdep check --from-path src --ignore-src --rosdistro melodic
```
出てきたパッケージをDockerfileに追加して、run.shを実行します。

## Dockerfile
```Dockerfile
# This is an auto generated Dockerfile for ros:ros-base
# generated from docker_images/create_ros_image.Dockerfile.em
FROM ros:melodic-ros-core-bionic

ENV http_proxy ${HTTP_PROXY}
ENV https_proxy ${HTTPS_PROXY}
ENV HTTP_PROXY ${HTTP_PROXY}
ENV HTTPS_PROXY ${HTTPS_PROXY}

ENV DEBIAN_FRONTEND=noninteractive
ENV DEBIAN_NOWARNINGS=yes

# install bootstrap tools
RUN apt-get -o Acquire::http::proxy=${http_proxy} update  && apt-get -o Acquire::http::proxy=${http_proxy}  install --no-install-recommends -y \
    build-essential \
    python-rosdep \
    python-rosinstall \
    python-vcstools \
    python-pip \
    && rm -rf /var/lib/apt/lists/*

# bootstrap rosdep
RUN sudo -E rosdep init && \
  rosdep update --rosdistro $ROS_DISTRO

# install ros packages
RUN apt-get -o Acquire::http::proxy=${http_proxy}   update && apt-get -o Acquire::http::proxy=${http_proxy}  install -y --no-install-recommends \
    ros-melodic-ros-base=1.4.1-0* \
    && rm -rf /var/lib/apt/lists/*

RUN apt-get -o Acquire::http::proxy=${http_proxy} update && apt-get -o Acquire::http::proxy=${http_proxy}  install -y --no-install-recommends \
    python-catkin-tools \
    #########################################################
    # ここに、rosdep checkで確認したモジュールを追加する
    #########################################################
    vim

RUN echo "source /opt/ros/melodic/setup.bash" >> /root/.bashrc
```

## docker-compose.yaml
IPアドレス(XXX.XXX.XXX.XXX)とポート番号(PPPP)は環境に応じて変更します。  
使用しているPCのグラフィックスカードがGeForceではないのでCUDAの設定はしていません。

```Dockerfile:docker-compose.yaml
version: "2.4"
 
services:
  Melodic:
    runtime: runc
    image: melodic_image_${USER}
    tty: true # make container stay launched
    container_name: melodic_${USER}
    privileged: true
    environment:
      # Display X Server GUI.
      - DISPLAY=${DISPLAY}
      - QT_X11_NO_MITSHM=1
      # Configure PROXY.
      - HTTP_PROXY=http://XXX.XXX.XXX.XXX:PPPP
      - HTTPS_PROXY=http://XXX.XXX.XXX.XXX:PPPP
    build: 
      context: ./
      dockerfile: ./Dockerfile
      args:
        - HTTP_PROXY=http://XXX.XXX.XXX.XXX:PPPP
        - HTTPS_PROXY=http://XXX.XXX.XXX.XXX:PPPP
    volumes:
      # GUI
      - /tmp/.X11-unix:/tmp/.X11-unix
      # H/W
      - /dev:/dev:rw
      # work
      - ./catkin_ws:/root/catkin_ws
    network_mode: host
    ports:
      # 外部と通信するポートを記載
      - "9090:9090"
      - "PPPP:PPPP"
    working_dir: /root/catkin_ws
```

起動スクリプトファイルです。
```sh:run.sh
#!/bin/bash
xhost + local:root
docker-compose up -d
docker-compose exec Melodic bash
```