Unity上のロボット部品を組み立てて，ROS2でコントロール

# やりたいこと
[箱庭](https://toppers.github.io/hakoniwa/)という新しいシミュレーション環境を作ろうと思ってはや４年．

おかげさまで，これまで色んなものを作って利用いただいてきましたが，箱庭らしさがまだないよねーっと思っていました．

箱庭らしさとして，不足していると思っているものは以下あたり．

1. ロボット自体を組み立てるとかできない
    * 自由な発想で，ほしいロボットを作って，手軽に動かしたい
    * 子供でも触れるようにしたい
2. 様々な人が仮想空間上で集まる空間がない
    * 例えば，クラウド/VR空間を作る
    * その空間で，作りたいものをワイワイ議論しながら作り上げていく感じ

やりたいことは多いですが，ひとまず，この記事では，「１」のロボット自体を組み立てるための環境づくりに注力して，話を進めます．

# できるようになったこと
TOPPERS/箱庭の成果物として，『[箱庭 ROS シミュレータ](https://github.com/toppers/hakoniwa-ros2sim)』があります．

この環境には，ROS2トピック通信可能な様々なロボット部品を用意されており，Unityエディタ上で自由にロボットを組み立てできます．

まだ部品群は少ないですが，基本的な仕組みはできたと思いますので，今後，部品を増やして，様々なロボットを作れるようにしたいと思っております．

本環境の導入手順はこちらを参照ください．

https://qiita.com/kanetugu2018/items/28c137b78ec6ec74033a

また，ロボット部品の組み立ては，「箱庭ロボット・コンフィギュレータ」で行います．

以下，「箱庭ロボット組み立てキット」という名前で導入手順を追記しています．

# ロボット部品群
箱庭ロボット組み立てキットに含まれる部品の種類としては以下の３種類あります．

* 基本部品
    * Unity/ArticulationBodyで作成された物理モデルです．
    * コライダも付いてます．
    * ROS2通信はできません．
    * 後述するアクチュエータとセンサをつなげるハブの役割をします．
* アクチュエータ
    * Unity/ArticulationBodyで作成されたモーター類です．
    * 必要に応じてコライダを付けています．
    * ROS2通信としてはsubscriptionがメインで，トピック受信して駆動します．
    * トピック名は部品パラメータで変更できます．
    * ROS2通信用のスクリプト群は箱庭ライブラリとしてアタッチされています．
* センサ
    * Unityライブラリを使用して作成されたセンサ類です．
    * 必要に応じてコライダを付けています．
    * ROS2通信としてはpublishがメインで，トピック送信し続けます．
    * ROS2通信用のスクリプト群は箱庭ライブラリとしてアタッチされています．

これらの部品群は，Unityプロジェクトの以下の「RobotParts」というフォルダ配下に格納されています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/e9b61756-7e7b-c98d-5ca7-f3b98fdffde4.png)


以下，簡単に部品の紹介をしたいと思います．

## 基本部品
基本部品は「01_basic」配下にあります．現時点では，以下のようなものがあります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/dadd8f8c-344e-1a17-2679-ea794e8dc88d.png)


## アクチュエータ
アクチュエータは「02_Actuator」配下にあります．現時点では，以下のようなものがあります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/59a4a55b-846a-6df5-3326-78b92ef189f6.png)

## センサ
センサは「03_Sensor」配下にあります．現時点では，以下のようなものがあります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/5658f674-8d90-8cfd-9591-edaa892afba3.png)

まだ少ないですが…，そのうち，カラーセンサ，タッチセンサなども追加したいと思っています．

# ロボット組み立て方法
様々な組み合わせでロボットが作れますが，今回はTurtleBot3風のロボットを組み立て方を紹介しながら，組み立て方法を説明してみたいと思います．

完成イメージはこうです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/034efbf7-eab7-4b3a-00c9-4eef0d0b0271.png)

## 前提
箱庭ロボット組み立てキットでロボットを組み立てするには，UnityのHierarchyビュー上のでドラッグ＆ドロップ操作します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/712718f8-5fdc-df3d-3492-2482cfdb3455.png)

上図のように，Hakoniwaというトップディレクトリ直下に，「env」と[Robot」があります．ロボット組み立てするには，「Robot」ディレクトリ直下に部品を配置する形になります．

## 最初の一歩
ロボットを作り始めるための最初の一歩としては，Projectビューの以下にある「RobotController」という部品をHierarchyビューのRobot直下にD＆Dします．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/be7f3391-50f6-6a6c-d75c-64b3957f3fc8.png)

D&Dすると，こんな感じになると思います．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/f41b089b-ab00-f272-6ba6-cacd0ff42462.png)

Robotディレクトリ直下に「RobotController」という名前のオブジェクトが配置されました．こいつが親玉であり，ロボットの名前となります．ロボット部品群はこの親玉の下にD&Dしていくことになります．

ちなみに，ロボットを増やす場合は，同様な方法でRobotディレクトリ直下にRobotControllerを配置すればよいです(名前は衝突しないようにリネーム必要)．

## 基本部品の配置
お次は，基本部品群を配置して形を整えていきましょう．
まずは，Projectビューの基本部品として，「MainBody」をRobotController直下にD&Dしましょう．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/87f995d2-9882-11ea-bc0a-61cb3058a650.png)

MainBodyには，ボディの前/上/下にそれぞれに，部品を取り付けるためのインタフェースを用意しています．

「前」は，以下の白い四角部分．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/031bc686-9886-a7c3-3fdc-8af935751708.png)

「上」は，以下の黒い四角が５個あります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/3bc8923c-8670-daef-45dd-ef886ed65c4d.png)

「下」は，以下の黒い四角が同様に５個あります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/aee079b2-c679-c588-10b0-f354cfe932f5.png)

Hierarchyビューで見ると，こんな感じです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/389f773d-f67e-fcdb-bdbd-95922e7b6eb1.png)

インタフェース名と位置は以下の通りです．
* Interface-F：前側
* Interface-UF：上の前側
* Interface-UB：上の後ろ側
* Interface-UR：上の右側
* Interface-UL：上の左側
* Interface-UC：上の中央
* Interface-DF：下の前側
* Interface-DB：下の後ろ側
* Interface-DR：下の右側
* Interface-DL：下の左側
* Interface-DC：下の中央

このまま，下の後ろ側(Interface-DB)に，キャスター付きの部品を付けてみましょう．今度は，「BodyBack」という部品をD&Dします．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/8a467966-83d4-2c2f-d7f3-b146506fcf32.png)

このまま，前側(Interface-F)に，カメラ設置用のホルダーを付けます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/d9d045b0-8920-b9e1-1f12-0d4f223376f4.png)

基本部品は以上です．お次は，アクチュエータを付けます．

## アクチュエータの配置
アクチュエータとしては，差動モータを配置しましょう．
Projectビューの02_Actuatorから，「DifferentialMotorController」をMainBodyの下前側(Interface-DF)にD&Dしましょう．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/52b547cf-6508-7ca4-8038-bbde1bc4a6cc.png)

そして，お次はタイヤです．左右取り付けましょう．

左のタイヤ(L-Wheel)をDifferentialMotorControllerの左側のインタフェース(Interface-L)にD&Dします．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/e570604a-016a-58f8-961f-11c4dc2ce178.png)

右のタイヤ(R-Wheel)をDifferentialMotorControllerの右側のインタフェース(Interface-R)にD&Dします．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/c17317f8-4a9b-1996-0cc2-3ae8542bcde2.png)

だいぶ，ロボットの形になってきましたね！

ここから，カメラを配置するためのサーボモータを２個配置します．
カメラは，左右と上下に動かして周囲監視できるようにしたいので，モータを２個付けるというわけです．

まずは，左右に動かすためのモータを付けます．ServoMotorBaseという部品です．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/25dd5fa1-d9a0-998b-4643-3f90ecf3f10c.png)

次に，この部品に対して，上下に動かすためのモータを付けます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/46498b0f-4893-aebf-7ef1-e21db68a4848.png)

これで，上下左右にカメラを動かすことができます．

## センサの配置
このまま，カメラを付けてみましょう．

Projectビューの03_Sensorから，「Camera」をServoMotorのインターフェースにD&Dしましょう．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/9a7216b8-9886-7ba6-817e-649dfb099512.png)

最後に，レーザスキャナをMainBodyの上に配置して終わりです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/bc4ee975-08bb-9be8-e084-ac9c9b114ca7.png)

できた！

# ROS2でのコントロール方法
さてさて，ここまではロボットの形しかないので，まだROS2で動かすことはできません．

ROS2で動かすために，箱庭環境では，ROS2用の様々なコンフィグレーションファイルやプログラムを自動生成できるようにしています．

箱庭ロボット組み立てキットで用意している箱庭環境では，様々な組み合わせで作られたロボット部品から，それらのコードを自動生成してくれます．ちょー便利ですね～．

そのための手順として，まずは，Unityエディタの[Window]->[Hakoniwa]->[Generate]をクリックします．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/fa413478-228e-0eff-c474-ac48f3583d85.png)

こうすると，以下のフォルダ直下に箱庭のROS2向けコンフィグファイル(RosTopics.json)が出来上がります．

* hakoniwa-ros2sim\settings\tb3

今回の場合は，こういう内容になります．

```json
{
  "fields": [
    {
      "topic_message_name": "RobotController_servo_base_angle",
      "topic_type_name": "geometry_msgs/Twist",
      "robot_name": "RobotController",
      "sub": true,
      "pub_option": null
    },
    {
      "topic_message_name": "RobotController_servo_angle",
      "topic_type_name": "geometry_msgs/Twist",
      "robot_name": "RobotController",
      "sub": true,
      "pub_option": null
    },
    {
      "topic_message_name": "RobotController_camera_image_jpg",
      "topic_type_name": "sensor_msgs/CompressedImage",
      "robot_name": "RobotController",
      "sub": false,
      "pub_option": {
        "cycle_scale": 100,
        "latch": false,
        "queue_size": 1
      }
    },
    {
      "topic_message_name": "RobotController_scan",
      "topic_type_name": "sensor_msgs/LaserScan",
      "robot_name": "RobotController",
      "sub": false,
      "pub_option": {
        "cycle_scale": 10,
        "latch": false,
        "queue_size": 1
      }
    },
    {
      "topic_message_name": "RobotController_cmd_vel",
      "topic_type_name": "geometry_msgs/Twist",
      "robot_name": "RobotController",
      "sub": true,
      "pub_option": null
    }
  ]
}
```

最後に，ここで作成したロボット(RobotController)を，ドラッグ＆ドロップで，以下の ``Resources/Hakoniwa/Robots`` に配置しましょう．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/a0394cdf-8973-22fb-6cf0-cf230c81d635.png)

ここから先は，従来のやり方と同じで，生成したコンフィグファイル(RosTopics.json)をベースに，dockerコンテナ上で，箱庭のコンフィグファイルを生成して終了です．

```
# bash hako-install.bash
###Phase3(config): Creating core_config
####Creating core_config
####Creating ros_topic_method
####Creating inside_assets
####Creating pdu_readers
####Creating pdu_writers
####Creating reader_channels
####Creating writer_channels
####Creating pdu_channel_connectors
####Creating unity_ros_params
####Creating pdu_config
####Creating param_world
####Creating tb3_parts
###Phase3(json): Success
Model is already installed.
Plugin is already installed.
Resources is already installed.
```

# シミュレーション実行方法

シミュレーション実行する前に，Project Settings の設定を一部変更する必要があります．

* Friction Type
    * Two Directional Friction Type
* Solver Type
    * Temporal Gauss Seidel

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/fa17a267-de27-dfd5-9354-8b76c7ee0581.png)

シミュレーション実行手順は，以下を参照ください．

https://qiita.com/kanetugu2018/items/28c137b78ec6ec74033a#%E7%AE%B1%E5%BA%AD%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%82%B7%E3%83%9F%E3%83%A5%E3%83%AC%E3%83%BC%E3%82%BF

ROS2制御プログラムの実行は，以下となります．

```
# bash run.bash tb3 RobotController
```

# デモ

https://twitter.com/kanetugu2020/status/1522764860849012737

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

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

https://hakoniwa.connpass.com/