ROS で TOPPERS/箱庭・単体ロボット向けシミュレータを動かす！

#概要
[TOPPERS/箱庭WG](https://toppers.github.io/hakoniwa/)では，[ROSConJP 2021](https://roscon.jp/)に向けて，ROS対応開発を全力投球で行っています！

現時点(2021/06/28)で，箱庭にROS機能を追加することに成功しましたので，簡単なデモを含めて，その内容を紹介します．


#本記事で伝えたいこと

* 箱庭の構造とその目的
* ROS機能をどのように箱庭コア機能に組み込むか悩んだ結果
* UnityのROS機能構造
* 箱庭上でROSプログラムを動作させるための手順

#箱庭の構造とその目的
何はともあれ，箱庭の構造を下図に示します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/f5f8c986-f58a-3074-0eca-a11f6b44f66c.png)

## ロボット制御プログラム
まず，一番上に，箱庭利用者が作成する「ロボット制御プログラム」があります．

制御プログラムの種類としては，現時点では，「ETロボコン向けの走行制御プログラム」や「ROSプログラム」等を視野に入れており，箱庭利用者が作成したプログラムを箱庭上で手軽に試せる環境構築を目指しています．

なお，箱庭WGの昨年の実績として，[ETロボコン2020](https://www.etrobo.jp/2020event-outline/)で，箱庭WGの要素技術の一部を利用頂きました．また，様々な教育現場で，オンラインロボット演習としても利用され始めています．

## 箱庭通信・アセット
ここで，シミュレータ上でこれらのプログラムを動かすためにはシミュレータと制御プログラム間で専用の通信が必要となります．この際にシミュレータの要件として，以下が挙げられると思います．

1. 制御プログラム側の改修なしで，透過的にシミュレータと通信できること．
2. 実機と同じ速度でのシミュレーションが可能であること(特に通信オーバーヘッドは低減したい)．

1 については，制御プログラムをそのまま実行できるシミュレータを用意することで対応する方法があります．ETロボコン向けには，そのためシミュレータとして，箱庭の要素技術であるマイコンシミュレータ[Athrill](https://www.toppers.jp/athrill.html)を利用しています．

一方で，ROS向けには，制御プログラムはROSノードとして実現されますので，ROSノードとロボットとのROS通信がそのまま利用できる必要があります([Gazebo](http://gazebosim.org/)のように)．

2 については，箱庭では用途に応じて通信方式を選択できるように，様々な通信方式(UDP,MMAP,ROS...等々)のサポートを視野に入れています．そのため，それらの通信方式のアセット化を検討しており，今回，UDP, MMAP に加えて ROS が追加された形になります．

## 箱庭ロボット・アセット通信インタフェース
箱庭は様々な通信方式を選択可能とする方針ですので，箱庭上で動作するロボットが通信方式に依存してしまうと保守性・拡張性が悪くなってしまいます．そこで，通信方式を隠蔽化するためのインタフェース層として，「箱庭ロボット・アセット通信インタフェース」層を用意しています．

## 箱庭ロボット・アセット向け通信データ(PDU)
通信方式の隠蔽化と同様に，箱庭では通信データ構造も隠蔽化することにしています．理由は先ほどと同じで，箱庭上で動作するロボットが通信方式/プロトコルに特化した通信データ構造になってしてしまうと，保守性・拡張性が悪くなってしまうためです．

そのための通信データとして，箱庭では抽象化された通信データ(プロトコルデータユニット(PDU))を用意しました．この通信データへのアクセスは，「メンバ名」とその「データ型」を指定してアクセスします．本構造にすることで，通信データがROSトピックデータ型であっても，UDP の RAW データであっても，同じ方法でデータの読み書きが可能になります．

## 箱庭ロボット・アセット
箱庭では，様々なロボットをユーザが色々と選んで手軽に動かすことができる環境を目指しています．

現時点では「LOGO Mindstorms EV3」でETロボコン向けの走行体モデル HackEV がありますが，ROSユーザ層向けにTurtleBot3の対応を進める予定です．

#ROS機能をどのように箱庭コア機能に組み込むか悩んだ結果

## 背景
現状，箱庭実装は Unity で実現しており，プログラミング言語は C# です．C# で作成している理由は，Unityスクリプトとの親和性を考慮してのことですが，設計上は Unity と箱庭とはインタフェースを切って直接依存しない構造にしています．Unity 非依存部の箱庭機能を「箱庭コア機能」と呼びます．これらのソースコードは，TOPPERSライセンスで以下で一般公開しています．

* 箱庭コア機能(Unity非依存部)
  * https://github.com/toppers/hakoniwa-core/tree/main/impl/asset/server/csharp
* 箱庭 Unity 依存部
  * https://github.com/toppers/hakoniwa-Unity-HackEV/tree/master/Assets/Scripts/Hakoniwa

箱庭にROS機能を取り込むための一番手軽な方法は，[Unity-Robotics-Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub) を利用することでした．

その他の方法として，ROS# を利用する手もあったのですが，こちらの実装は WebSocket ベースでしたので，通信オーバーヘッドが気になり採用しないことにしました．

## 悩んだこと
大枠，箱庭ROS実装として利用する技術は決まったのですが，細かな点で以下のよう悩みがありました．

* ROSユーザが Gazebo だけでなく，箱庭を使いたいと思ってもらえるようなメリットを作れるか？
* 箱庭実装が Unity の ROS 機能に依存してよいか？
* 箱庭のシミュレーション時間同期機構はどうするか？

## 悩んだ結果

### ROSユーザが Gazebo だけでなく，箱庭を使いたいと思ってもらえるようなメリットを作れるか？
当然ですが，ROSユーザが箱庭を使いたく理由がわかりませんでした．箱庭WGは結成３年目でまだヨチヨチ歩き状態です．
とはいえ，箱庭は Unity ベースでの実装も多く，Unity のメリットがそのまま箱庭にも適用されると思いました．そんな中，色々と世の中の話を参考にしたところ，以下のような記事もあり，一旦は，このまま進めようと思い至りました．

https://note.com/unityjapan/n/n1d3f0bc6f8fa

なお，個人的に，Unityで好きなところは，「直観的にサクサク作れる」，「ビジュアライズが綺麗」，「アセット・ストア色々ある」です．

### 箱庭実装が Unity の ROS 機能に依存してよいか？
ソフトウェアエンジニアとして，特定の技術に依存したコーディングはなるべくしたくないと思うのはしょうがないことと思います．技術は進化していくものですし，様々な技術に柔軟に適応できる設計を考えて，コーディングすることはとても楽しいです．

まー，そういう理由で，箱庭としては，Unity 側で提供されている ROS 機能をどうやって箱庭コア機能に取り込んで，Unity依存部のコーディング量をいかに減らすかがとてもチャレンジングな悩みでした．

結果として，以下のような切り分け方にすることにしました．

* 箱庭コア機能側(結構大改修でした)
  * ROS向けに箱庭アセット・ロボット通信インタフェース仕様の見直し/再実装する
  * ROS向けに箱庭ロボット・アセット向け通信データ(PDU)の見直し/再実装する
* Unity 依存部
  * Unity 向けの ROSトピックデータ型依存部を局所化(2クラスに集約)
  * Unity 向けの ROS 通信 APIの呼び出し部を局所化(1クラスに集約)
  * Unity 向けの ROSトピック依存クラスは自動生成可能にする

### 箱庭のシミュレーション時間同期機構はどうするか？
複数のシミュレータが連携してシミュレーションする場合，各シミュレータのシミュレーション時間を同期する必要があります．箱庭WGでは，分散型でスケール可能な同期方式を検討していますが，これをROS対応時にやるかどうか悩みました．

悩みの起点は，現状の箱庭の時間同期方式は，シミュレータ間を前提としている点です．ROSの場合，ROSノードの実行体は本物の機器(Linuxマシン等)になりますので，想定とは異なってしまいます(箱庭が実時間同期する仕組みはまだ未検討状態です)．

実時間同期の検討をするのはちょっと骨が折れそうでしたので，今回は，安直に箱庭上のロボットのシミュレーションとROSノードの時間同期は行わず，まずは動かすところから始めることにしました．


#UnityのROS機能構造
ここで，Unity の ROS 機能構造について概要を説明します(ROSトピックのみ)．

[Unity-Robotics-Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub)では，ROS通信向けに以下が公開されています．

* [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector)
* [ROS-TCP-Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint)

これらの機能の関係性を簡単に図示すると以下になります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/2820cfc8-f228-d29a-9a9d-53f5f8b3779f.png)

本機能は，Unity から直接 ROS 側とつながるものではなく，Unity-ROSエンドポイント(ROS-TCP-Endpoint)がUnity側のROSトピックメッセージの通信を仲介する構造になっています．

一方で，Unity側は，ROSトピック通信を可能にするUnity-ROS通信APIが提供されており，ROSトピックデータは，Unity 用の C# クラス(Unity-ROSメッセージ)としてアクセスします．また，Unity-ROSメッセージは，Unity のプラグイン機能を利用することで ROSメッセージから自動生成可能です．

#箱庭でのROS通信処理
Unity の ROS 機能に対して，箱庭 ROS 通信処理を設計検討した結果は以下の通りです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/700b56df-ea77-8caf-e33a-62d63148e3a7.png)

「オレンジ」部分が Unity 依存部で，「グリーン」部分が箱庭コア(Unity非依存部)側です．
この中で，ROS通信処理側は，ROSトピックのデータ型，トピック名に依存してプログラミングコードが変化します．以下，その内容を説明します．

* Unity-ROS 通信API
  * ROSトピック購読APIは，ROSトピックのデータ型とトピック名に依存しています．
* Unity-ROS メッセージ
  * Unity-ROS メッセージは，ROSトピックのデータ型の個々の要素に依存しています．
* PDU-ROS データ変換
  * PDU-ROS データ変換では，Unity-ROSメッセージを PDUに変換する必要があるため，ROSトピックのデータ型に依存しています(出版/購読でそれぞれ変換の方向は逆になります)．


#箱庭上でROSプログラムを動作させるための手順
上述の通り，ROSトピック依存クラス群を箱庭のために手作業で作成するのは手間となってしまいますから，あまり手間にならずに済む手順の検討結果を下図に示します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/727d7d99-8e4e-6a0b-407b-b027eeb216c2.png)

ROSメッセージ型を起点にして，ほぼ自動生成可能になっています．

## Unity側
まず，Unity側のUnity-ROSメッセージのコードに関しては，Unity のプラグイン機能でGUI上で自動生成します．

##箱庭側
箱庭側で必要となる「PDU-ROSデータ変換」，「Unity-ROSコールバック関数」は，それぞれ jinja2 のテンプレートを利用して自動生成します．

これらのテンプレートの入力は，「ROSメッセージ型(JSON)」と「ROSトピック情報(JSON)」です．

* 「ROSメッセージ型(JSON)」については，ROSメッセージ型から自動生成します．
* 「ROSトピック情報(JSON)」については，利用するトピック情報を手作業で作成します．

##コード例
上記で説明されているコード例は以下で公開しています．

* [ROSメッセージ型](https://github.com/toppers/hakoniwa-ros-samples/tree/main/ros2/workspace/src/ev3_msgs/msg)
* [ROSメッセージ型(JSON)](https://github.com/toppers/hakoniwa-ros-samples/tree/main/ros2/ros_json/ev3_msgs)
* [ROSトピック情報(JSON)](https://github.com/toppers/hakoniwa-ros-samples/blob/main/settings/ev3_tb3/RosTopics.json)
* [変換テンプレート](https://github.com/toppers/hakoniwa-ros-samples/tree/main/utils/template)

#デモ
本デモでは，TurtleBot3とETロボコン向けのロボット HackEV を ROSプログラムで制御しています．

以下で公開中です．

https://github.com/toppers/hakoniwa-ros-samples


# お知らせ：箱庭もくもく会

TOPPERS箱庭WGでは，もくもく会を不定期に開催しています．
本記事の内容を実際に試してみたい方や試してみたけど詰まっている方は，ぜひご参加ください．画面共有しながら参加者同士でサポートしたり，一緒に新しい機能を実装したりと”もくもく”しましょう．

https://hakoniwa.connpass.com/