Unity＋Python＋箱庭で自作ドローンを動かしてみる！

Unityでドローンを使ったシミュレーションするには、[AirSim](https://microsoft.github.io/AirSim/Unity/)が有名ですが、これを[TOPPERS/箱庭](https://toppers.github.io/hakoniwa/)でやってみたら、案外それっぽいのができちゃったので記事にしてみました。

* 2023/11/12: 追加情報
  * 最新の箱庭ドローンシミュレーション環境の情報です！
  * [C言語で作ったドローン物理モデルをUnityでビジュアライズ＆制御する！TOPPERS/箱庭のドローン・シミュレーション構想と現在の開発状況](https://qiita.com/kanetugu2018/items/c3e34b35fc7afb17e196)

# きっかけ

この論文です。

https://www.cst.nihon-u.ac.jp/research/gakujutu/61/pdf/G-30.pdf

この記事を読んで、『Unityで、プロペラ部分を RigidBody で作って、上向きの力を外部から与えれば簡単に実現できそう！』という思い付きからでした。


# やってみた結果

こんな感じのものが簡単に作れました。

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

見た目はイマイチですが、本質的なところは理解できたと思った瞬間でした。
ドローンって、こうやってうごくんやーって納得感ありました。

# TOPPERS/箱庭に組み込む

最近、TOPPERS/箱庭では、Unity側の大規模リファクタリングしてます。方向性としては、箱庭上で動くロボットを、プラグインとして簡単に組み込む仕組みにしていこうというものです。

現状の成果としては、TurtleBot3 があります。

https://github.com/toppers/hakoniwa-unity-tb3model

ちなみに、これを利用して [OpenEL](https://www.jasa.or.jp/openel/) で動かせるようにしたものがこちらです。

https://github.com/toppers/hakoniwa-openel-cpp

この方向性で、Unityで作成したドローンを箱庭プラットフォーム上で動かせるようにしたいと思いました。

# 箱庭のドローン一般公開リポジトリ

これです。

https://github.com/toppers/hakoniwa-unity-drone-model

## ドローン

作成したドローンは以下です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/e5977810-e572-d5bf-61dd-8c7e37aae164.png)


センサとしては、以下があります。

* レーザスキャナ([sensor_msgs/LaserScan](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/LaserScan.html))
  * 360度角で、１度単位で距離情報を取得できます。
* カメラセンサ([sensor_msgs/CompressedImage](http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CompressedImage.html))
  * 640×480の jpeg形式のデータを取得できます。
* IMU(sensor_msgs/Imu)
  * 姿勢角度(クォータニオン)
  * 角速度
  * 加速度

各モーターへの指示としては、一旦、以下としました。

* データ型
  * [geometry_msgs/Twist](http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/Twist.html)
* 指示
  * linear/x
    * 横方向への移動指示：-1 ～ 1の値
  * linear/y
    * 前方向への移動指示：-1 ～ 1の値
  * linear/z
    * 縦方向(上昇/加工)への移動指示：-1 ～ 1の値
  * angluar/x
    * ドローンの水平方向の方向転回指示：-1 ～ 1の値

上記のモーター指示に対して、論文にある力を各モーターに指示をします。
ただし、方向転回については、ドローンの中央にある RigidBody に対して、方向転回指示をします。

## このリポジトリで出来ること

以下をお試しできます。

* ドローンのUnityモデルのデバッグ
  * Unity単体で、キーボードでドローンを操作できます。
* Python制御
  * Unity上のドローンをPythonプログラムから制御できます。
 
# 前提とする環境

* ドローンのUnityモデルのデバッグ
  * Windows/Mac/Linux で利用可能です
* ドローンのPython制御
  * Windows 10/11 および WSL2 でのみ利用可能です
 
# インストール方法


TOPPERS/箱庭のドローンアセットのリポジトリを以下のようにクローンします。

```
git clone --recursive https://github.com/toppers/hakoniwa-unity-drone-model.git
```

クローン終わったら、以下のようにディレクトリ移動します。

```
cd hakoniwa-unity-drone-model/
```

次に、インストーラを実行します。

```
bash install.bash
```


成功すると、DONEというメッセージが出ますので、この状態で Unity Hub で当該プロジェクトを開きましょう。

対象フォルダ：hakoniwa-unity-drone-model\plugin\plugin-srcs

成功するとこうなります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/8891baed-3c95-eb46-1394-f0cad80f1506.png)

なお、Unityエディタのバージョンによっては、起動中にエラーとなる場合があります。 その場合、途中、ダイアログがポップアップされて警告されますが、気にせず起動しましょう。

原因は、`Newtonsoft.Json` が不足しているためです。 対応方法は、下記記事にある通り、Unityのパッケージマネージャから `Newtonsoft.Json` をインストールすることで解消できます。

https://qiita.com/sakano/items/6fa16af5ceab2617fc0f

# docker イメージの取得
Python 制御する場合は、docker イメージの取得が必要です。
※Python制御しない場合は、このオペレーションは不要です。

箱庭では、WSL2にインストールされた docker を使いますので、事前にインストールされている必要があります。 また、docker を利用する局面では、必ず、以下のように docker を起動してください。

```
sudo service docker start
```


まずは、`hakonwia-base` へ移動します。

```
cd hakoniwa-base
```

そして、docker イメージをインストールします。

```
bash docker/pull-image.bash runtime
```

成功するとこうなります。

```
$ docker images
REPOSITORY                              TAG       IMAGE ID       CREATED        SIZE
toppersjp/hakoniwa-ai-runtime           v1.1.0    53fb2f7aedba   6 hours ago    4.54GB
```

# ドローンのUnityモデルのデバッグ

Unityエディタ起動後、プロジェクトビューの　Scenes/Drone/DroneWork をダブルクリックしてください。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/93c575ba-afd5-e4f4-bcfe-5c2c9a4d22f1.png)

この状態で、[Window]->[Hakoniwa]->[GenerateDebug]をクリックします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/eea5951b-e71f-c299-b091-aca1bbe64091.png)

この後、Unityの `再生ボタン` をクリックすると、下図のようになり、キーボードでドローンを動かすことができます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/bcf710ad-882c-f397-a923-59a0b7eda76e.png)

* 上昇
  * スペースキー
* 下向
  * `A` キー
* 右移動
  * 右矢印キー
* 左移動
  * 左矢印キー
* 前方移動
  * 上矢印キー
* 下方移動
  * 下矢印キー



# ドローンのPython制御

## Unity 側の準備

Unityのプロジェクトビューの　`Scenes/Hakoniwa` をダブルクリックしてください。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/2d2728dc-5c1f-c407-0313-53ef93b662f5.png)

お気づきかもしれませんが、インストール直後は ドローン は存在していません。 代わりに、SampleRoboが配置されています。

これを ドローン に差し替えましょう。

まず、ヒエラルキービューの Robot 配下に、SampleRobo が見えると思います。これを削除しましょう。 対象のオブジェクトを選択して、右クリックして Delete を押下することで削除できます。

次に、プロジェクトビューの　Prefab/Robots をクリックすると左側に `Drone` があります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/cd67fcc2-f48a-9a32-ba88-220c651b7016.png)

これをドラッグして、ヒエラルキービューの Robot の下でドロップします。成功するとこうなります。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/2bf3229a-04d2-3a65-3708-b301213f783f.png)

この状態ですと、床に埋まっていますので、ドローンの位置調整をしてあげましょう。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/0038c90d-2888-a0b8-94f1-f0548dd0f228.png)

※緑の矢印（Y軸）をクリックしたまま上に移動させるだけです。

あと、下図のように、ヒエラルキービューで、`Drone`をクリックして、インスペクタ―ビューを出して、`EnableEx Ctrl`にチェックを入れてください。これをしないと、Pythonから制御できません。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/70263fb4-9615-026f-99ef-75d64d765527.png)

最後に、`Window/Hakoniwa/Generate` をクリックして終了です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/d2a65687-7d14-8330-1c99-f499082b42ef.png)

この `Generate` の操作は、Windowsを再起動した場合は必ず１回は実行してください。

## Python 側の準備

Visual Studio Code 等で、サンプル提供しているPythonプログラムを開いてください。

* 場所
  * hakoniwa-unity-drone-model/hakoniwa-base/workspace/dev/ai/sample_drone.py

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/1bcb676a-75fc-3a51-41c7-0ec895cf06e5.png)

姿勢制御部分はこのあたりですが、水平方向の制御しかやっていません。。（今後に期待）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/0022e464-3d15-2aea-2fb0-48671fee15db.png)

必要に応じて、このプラグラムを修正していただければ、ドローンを適切に動かすことができると思います。

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

本リポジトリの以下のディレクトリに移動します。

```
cd hakoniwa-base/
```

以下のコマンドで箱庭のランタイムおよび Python のサンプルプログラムを起動します。

```
bash docker/run.bash runtime
```

起動ログ：
```
ASSET_DEF=asset_def.txt
INFO: ACTIVATING MOSQUITTO
[93243.755874]~DLT~    9~INFO     ~FIFO /tmp/dlt cannot be opened. Retrying later...
INFO: ACTIVATING HAKO-MASTER
OPEN RECIEVER UDP PORT=172.25.195.216:54001
OPEN SENDER UDP PORT=172.25.195.216:54002
mqtt_url=mqtt://172.25.195.216:1883
PUBLISHER Connecting to the MQTT server...
PUBLISHER CONNECTED to the MQTT server...
delta_msec = 20
max_delay_msec = 100
INFO: shmget() key=255 size=80768
Server Start: 172.25.195.216:50051
INFO: ACTIVATING :dev/ai/sample_drone.py
START DRONE TEST
LOADED: DroneTestDriver
LOADED: Drone
INFO: Drone create_lchannel: logical_id=1 real_id=0 size=48
subscribe:channel_id=0
subscribe:typename=Imu
subscribe:pdu_size=432
subscribe:channel_id=2
subscribe:typename=LaserScan
subscribe:pdu_size=3044
subscribe:channel_id=3
subscribe:typename=CompressedImage
subscribe:pdu_size=1229064
WAIT START:
```

この状態で、Unityの再生ボタンをクリックします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/33bf0241-18e9-f868-8e91-821a8a14de1a.png)

このように、ドローンが見えると思いますので、`START` ボタンをクリックするとシミュレーション開始します。

シミュレーション終了する場合は、Unityの再生ボタンを止めて、起動していた WSL2のターミナルを　CTRL+C で停止してください。


# デモ
Pythonから定期的にファイル(cmd.txt)から読み込んでおり、ここに水平方向の目標姿勢角度指示値が入っていますので、その値を変えてドローンを動かしています。


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