Houdiniからトイドローン(Tello)を動かしてみる

こちらは[Houdini Apprentice Advent Calendar 2019](https://qiita.com/advent-calendar/2019/happrentice)の22日目の記事。
習作の予定だったがHoudiniをゴリゴリ触ったというよりか、Houdini/トイドローンの接続に悪戦苦闘したという内容になってしまった。。



#完成形
言葉で説明する前に、例によってとりあえずGIFを貼り付け。
<img width="700" alt="tello-flight" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/8cef78ba-9e59-bfa7-5147-2e26498836c7.gif">
（撮影動画＋透過の画像キャプチャの合成）

この後紹介する【 記述編 -基礎- 】で作成したもの。
要は **Houdini側で作成した軌跡に沿うようにトイドローンを飛ばした**、という感じ。うーん、わかりにくい。


　
#前提編
###何を作ったのか
この動いているドローンは下記の仕組みから構成されている。

`(1) Houdini(VEX) : ランダムにxyz方向のいずれかに1移動、を繰り返す`
`(2) Houdini(Python) : (1)をドローンへの命令に書き換え、(3)に渡す`
`(3) Tello(Python) : DJI-SDKにて公開されているサンプルプログラムの一部を書き換え、(2)から受け取った命令を実行`
`(4) Houdini(Python) : (3)のPythonコードをHoudini内でimportして実行`

(1)については過去にHoudiniで作成した**Random Pipe Generator**で代用。
冒頭の表現を言い換えれば、今回は(2)に大半の時間を費やしたという感じ。


※参考URL
・[Houdiniでスケルトン天井のパイプのベースを自動生成する(*Qiita)](https://qiita.com/shin_t_o_/items/7db2699cf395cffd63fd)

###何がしたかったのか
最近しばしば見かけるこれら↓
<blockquote class="twitter-tweet" data-lang="en"><p lang="ja" dir="ltr">インテルは世界中の花火をDroneに置き換える…だそうです。花火職人はDroneがライバルになるなんて考えもしなかっただろうな。Droneが石油施設の爆破に使われるのは嫌だけど、こっちは大歓迎です。<br> <a href="https://t.co/nvJVsZMRSQ">pic.twitter.com/nvJVsZMRSQ</a></p>&mdash; セキ ヤスヒサ🌖 (@Campaign_Otaku) <a href="https://twitter.com/Campaign_Otaku/status/1178860468867256322?ref_src=twsrc%5Etfw">October 1, 2019</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
<blockquote class="twitter-tweet" data-lang="en"><p lang="ja" dir="ltr">LEDを取り付けた多数のドローンをコンピュータで制御し、夜空で光のショーを行う企業が米国などで活動中。半導体で有名なインテルもその一つだが、精緻な作品で定評を得つつあるのがデトロイトの Firefly 社。同社の作品の一部を紹介した動画。100台のドローンが大活躍。<a href="https://t.co/hDVuSd0aaN">pic.twitter.com/hDVuSd0aaN</a></p>&mdash; Oguchi T／小口 高 (@ogugeo) <a href="https://twitter.com/ogugeo/status/1197892546929684480?ref_src=twsrc%5Etfw">November 22, 2019</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
のような近未来感あふれるドローン編隊飛行、Houdiniで軌跡を描いたら綺麗なのでは…？と思ったのがきっかけ。（実際どうなんですかね？幾何模様であれば定式化してドローン側に渡してあげるだけでしょうし、対応言語次第なのでしょうか）



###トイドローン(Toy Drone)とは

明確な定義があるわけではないが、

 - 1～2万円で購入可能
 - 軽量
 - 操作が簡単
 - モノによってはパーツ拡張にも対応
 - モノによってはプログラムから操作することも可能（ScratchやPythonなど）

辺りを満たす、子供～大人まで幅広く遊べるドローンのこと、といったところか。
今回お試しで使ってみたTelloは、スマートフォンアプリ経由で接続すればアプリ画面をコントローラにして「ドローンについたカメラ」が見ている景色が見える。

<img width="700" alt="tello" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/13c1aa4c-256d-5d03-0cfc-5a81f66e0ef4.png">

<img width="700" alt="tello-app" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/7b69f445-c976-4d50-d00a-14ba4f03e22f.jpeg">

今回使用したトイドローンTello 2台のスペック概要は下記の通り。
いま買うなら[DJI Mavic Mini(*website)](https://www.dji.com/jp/mavic-mini)辺りだと思うので、あくまで簡易的な紹介。

|項目|1台目詳細|2台目詳細|
|:---:|:--:|:--:|
|商品名|Tello Boost コンボ|Tello EDU|
|価格|￥13,750|￥17,050|
|サイズ|98×92.5×41 mm|98×92.5×41 mm|
|重量|81.6 g|87 g|
|最大飛行時間|13 min|13 min|
|最大飛行高度|30 m|30 m|
|写真|5MP (2592x1936)|5MP (2592x1936)|
|動画|HD 720p30|HD 720p30|
|SDK|1.3|2.0|
後ほど触れる＆詳しい方はお分かりの通り、通常のTelloとTello EDUはSDKのバージョンが異なり、編隊飛行やミッションパッド認識などに対応しているのはTello EDUのみ。お、不穏な空気だな？

※参考URL（回し者じゃないです）
・[Tello(*website)](https://www.ryzerobotics.com/jp/tello)
・[Tello EDU(*website)](https://www.ryzerobotics.com/jp/tello-edu)

　
#準備編

###実行環境
OS : Windows 10
Houdini : Houdini Apprentice 17.5.460
Python : 2.7.15 (by pyenv) -> 2.7.15 (Houdini python)
Tello : Tello + Tello EDU

###Telloサイド
スマートフォンアプリ経由での飛び方は把握している前提で、TelloをPythonから操作することを考える。
アプリで接続すればもうカメラ画像はアプリに同期されるし、すぐ飛び立てるのですごい。かがくのちからってすごい。

幾つか接続方法はあるが、今回はDJIが公開しているSDKである[Tello-Python(*GitHub)](https://github.com/dji-sdk/Tello-Python)を使用する。
ここも含めて詳細に書き始めると記事が肥大化してつらい（正直）ので、すでに詳細に解説されている記事を参考のこと。

※参考URL
・[DJI公式SDK「Tello-Python」を試そう](https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c)
・[PythonによるTello操作（基本、及びクラウドからのMQTTによる操作まで）](https://qiita.com/makotaka/items/e3ed92e58e9bc15cefbf)


###Windowsサイド
少し前であればLinux環境での実行推奨だったようだが、現在はWindowsでも動くので挙動確認時はWindowsを選定。ただしTello-PythonはREADMEにもあるように`based on python2.7`のため※、pyenvで切り替えて実行していた。

※
一応3系でやれないこともないが、`ImportError: No module named '_curses'`エラーが生じる。[こちら(*stackoverflow)](https://stackoverflow.com/questions/35850362/importerror-no-module-named-curses-when-trying-to-import-blessings)を参考に

```python
pip install windows-curses
```

と入れてあげればモジュール的には解決するが、今度は2/3系の書き換えが必要そうなエラーが吐かれたためこれ以上確認していない。Houdini内pythonは幸か不幸か2.7系のため、バージョン切り替えは特に気にしなくとも良さげ。

【 記述編 -応用- 】ではHoudini内Pythonに別途モジュールをインストールする必要があったので後ほど触れる。

###Houdiniサイド
特定のファイル（今回であれば【dji-sdk/Tello-Python】や【TelloSDK/Multi-Tello-Formation】）を実行対象として読み込みたかったので、`houdini.env`内のPYTHONPATHにディレクトリパスを追記する。


　
# 記述編 -基礎-

###(1)Houdini(VEX)
==================================
既に記載した通り、トイドローンに這わせたい軌跡は過去作成済みのRandom Pipe Generatorでいう
[「グリッド分割された指定空間内で始点・終点を決め、最短経路までlineを伸ばすサブネット」(*Qiita)](https://qiita.com/shin_t_o_/items/7db2699cf395cffd63fd#%E5%AE%9F%E8%A3%85)
を使いまわす。

ただ(2)でHoudini Pythonに渡すため、アトリビュートに格納しておく。


###(2)Houdini(Python)
==================================

Houdini側での座標1移動を、Tello側での20cm移動に対応させることを考える。
どうやらtello python commandの最低移動距離は20cmかららしい。
ただ変換するのもつまらないので、文字通り「ヒネリ」を加えてみる。

「上昇して右移動」「前進して右移動」のときに限り、移動ではなくFlipで置き換えてみる。

|VEX|Tello-Python|
|:---:|:--:|
|*** → (1, 0, 0)|forward 20|
|*** → (0, 1, 0)|up 20|
|(1, 0, 0) → (0, 0, 1)|flip r|
|(0, 1, 0) → (0, 0, 1)|flip r|
|(0, 0, 1) → (0, 0, 1)|right 20|
※「2回以上続けて右移動」のときだけflipナシ

ただのIF文ラッシュだが、、これをpythonで書き下す。

```python:PythonScript（Houdiniノード）
node = hou.pwd()
geo = node.geometry()


# 使いまわしアセット
#  ->「xyzいずれかの方向に1移動」を0～2に対応させ、randVecというattributeに格納したもの
randVec = geo.intListAttribValue("randVec")

# 実行用command listを作成
# 基本は「その方向に移動」だが、ちょっとひねりでflipを混ぜてみている
command = []

for i in range(1, len(randVec)):
    if randVec[i] == 0:
        command.append("forward 20")
    elif randVec[i] == 1:
        command.append("up 20")
    elif (randVec[i] == 2) & (randVec[i-1] != 2):
        command.append("flip r")
    else:
        command.append("right 20")
```

###(3)Tello(Python)
==================================
公開されているTello-Pythonは送信したい動き＝コマンドをまずは`command.txt`に記述。
かつ本体である`tello_test.py`実行時に引数として渡すようになっている。

<img width="700" alt="tello-program" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/df489f77-7d1b-9fcd-01b0-228ca282c6e9.png">


今回は実行上の都合※により`tello_test.py`は関数化して引数にcommandのリストを受け取るようにする。具体的には

```python:tello_test.py
from tello import Tello
import sys
from datetime import datetime
import time


# command.txt読み込み
start_time = str(datetime.now())
file_name = sys.argv[1]
f = open(file_name, "r")
commands = f.readlines()

tello = Tello()
for command in commands:
    if command != '' and command != '\n':
        ...
```

を

```python:tello_test.py(modified)
def tello_command(command):
    from tello import Tello
    import time

    tello = Tello()
    for command in commands:
        ...
```

と書き換えるイメージ。
そして `tello_test.py` をHoudini内pythonに記述してHoudini内から `tello.py` を呼ぶ。

※
Houdini内で`command.txt`を引数としてpythonファイルを実行する方法が解らなかったというだけの話。。

###(4)Houdini(Python)
実行タイミングをこちらで制御したかったため、NULLノードにRunボタンを実装。
(2)と(3)を用いて取得したリストをtelloコマンドに変換し、「tello起動プログラムに渡す」ボタンとする。

具体的にはこういうもの（何処かのサンプルファイルで見かけた）。
<img width="700" alt="run-button" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/70270690-c8fb-183b-3bfb-2bf181326d01.png">


###実行
冒頭のGIFの通り。申し訳程度に、Houdiniでの画像キャプチャを透過して動画に合成表示している。
完全一致…ではないものの、狙った挙動はできているっぽい。


　
#記述編 -応用-
一応Houdini経由でTelloを飛ばせたは良いが、これ別にHoudiniを経由する必要がないやんけ・・・
IF文ラッシュで作成したcommandのリストを`command.txt`にコピペしてしまえばすぐに同じことが再現できてしまう。ということで編隊飛行とかスパイラル軌跡みたいなもうちょっとHoudiniらしいことをしよう。

　
**…先に結論から言うと、あんまりうまくいかなかった。** 理由は下記の通り。

- 編隊飛行にはSDK2.0対応のTello EDU版が必須  
（用意していたうち1台がこれを満たさない）（とだいぶ書き進めてから気づく）
- Tello SDKが斜め移動に対応していない  
（これもだいぶ書き進めてから気づく）（事前準備って大事だ）

とはいえ或る程度見通しは立ったので、出来たところまでは記述しようかと。


###編隊飛行用のモジュールに切り替える
上記で使用したTello-Pythonは編隊飛行に対応していないため、[TelloSDKのMulti-Tello-Formulation(*GitHub)](https://github.com/TelloSDK/Multi-Tello-Formation)を選定した。基本的にはTello-Pythonと似ており、実行時に `command.txt`を渡すところも同じだったので同様に書き換える。

そしてREADMEにもある通り `pip` や `netifaces`，`netaddr`のパッケージインストールが必要。

```python:HoudiniPython
python -m pip install netifaces
python -m pip install netaddr
```

ここにだいぶ時間を費やしてしまった…Houdini内Pythonにパッケージインストールをするうまい方法が浮かばず、最終的にWindowsの環境変数にHoudiniPythonを追加してcommand lineで対応。しかもnetifacesがクセモノでインストールエラー連発。

`error: Microsoft Visual C++ 14.0 is required`
のエラーに対して最終的にはVisual C++ 2015 Build Toolsのインストールで解決（2019版ではダメだった）。
かつpip install時に `--user`オプションを付与して無事環境構築完了。

※参考URL
・[Pip error: Microsoft Visual C++ 14.0 is required(*stackoverflow)](https://stackoverflow.com/questions/44951456/pip-error-microsoft-visual-c-14-0-is-required)


###プリミティブな軌道に沿って編隊飛行させてみる
解説するほどのものではないけれど、chrampを用いて半径が途中で変わるようなspiralを書いてみる。
<img width="700" alt="spiral-vex" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/4ff49e7f-d1fb-5c08-7171-9f77a96cd0c5.png">

```python:VEX
float p = float(@ptnum) / (@numpt - 1);
float ang = radians(p * 360 * chi("spiralNum"));
float radi = ch("radi");
radi *= chramp("parm", p);

@P.z = radi * cos(ang);
@P.x = radi * sin(ang);
```


###これをどのようにコマンド変換するか
さて今度はxyz軸上のグリッド移動、という簡単なことにはならなそう。ということでこんな風に落とし込んでみた。
各点の座標を $P[i]$，移動ベクトルを $V_{i}$，移動ベクトルを正規化したものを単位ベクトル $e_{i}$とし、移動＋[ヨーイング角(Wiki)](https://ja.wikipedia.org/wiki/%E3%83%A8%E3%83%BC%E3%82%A4%E3%83%B3%E3%82%B0)のみ回転で動かすことを考える。HoudiniはY-upなので、懐かしの余弦定理から回転角 $\theta$は下記のように書ける。
<img width="700" alt="figure" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/dace103f-fead-65e5-2bbd-ef50592c5e4c.png">
※ヨー角回転＝首を横に振る方向の回転、のイメージ。平面回転というべきか。

### 書き下す
```python:VEX
# 単位ベクトルをVEX側でアトリビュートとして作成しておく
v@normalize = normalize(point(0, "P", @ptnum+1) - point(0, "P", @ptnum));
```
↓　↓　↓　↓

```python:PythonScript（Houdiniノード）
import numpy as np
import math


node = hou.pwd()
geo = node.geometry()

# すべての点を取得
points = geo.iterPoints()

pos = []
norm = []
deg = []


# 移動／回転用の配列を先に計算
for point in points:

    # 各pointの座標を取得
    pos.append(point.attribValue("P"))
    # 各pointにおける接線（単位）ベクトルを取得
    norm.append(point.attribValue("normalize"))
    
for i in range(len(pos) - 1):
    # 単位ベクトルの平面成分を取得したもの
    e_xz_i0 = np.array([norm[i][0], norm[i][2]])
    e_xz_i1 = np.array([norm[i+1][0], norm[i+1][2]])
    # 各point間移動時のyaw回転角度を取得
    radi = math.acos( np.dot(e_xz_i0, e_xz_i1) / np.linalg.norm(e_xz_i0)*np.linalg.norm(e_xz_i1) )
    deg.append(math.degrees(radi))


# normベクトルの何倍をcm換算で移動させるかを定義しておく
dist = 50

# ---------------------------
# command送信パート（定型コマンド）
# ---------------------------
command = []
# 複数台動かしたい場合はここでスキャン数を定義
command.append('scan N')
# 飛ばしたい台数分の型番をメモ（事前にformulation.pyで動作確認が必要）
command.append('1=***********')
command.append('2=***********')
...
command.append('テンプレートコマンド')

# ---------------------------
# command送信パート（軌跡依存コマンド）
# ---------------------------
for i in range(len(pos) - 1):

    # 機体を回転したら単位ベクトルの向きも変わるため回転行列を掛けていく
    forward = - norm[i][0] * np.cos(deg[i]) - norm[i][2] * np.sin(deg[i])
    left = norm[i][0] * np.sin(deg[i]) - norm[i][2] * np.cos(deg[i])
    
    # [dist]倍しても20cmに満たない時は20cmを採用する
    command.append('*>ccw ' + str(round(deg[i])))
    command.append('*>forward ' + str(max(round(forward * dist), 20)))
    command.append('*>left ' + str(max(round(abs(left) * dist), 20)))
    command.append('*>up ' + str(max(round(norm[i][1] * dist), 20)))

command.append('*>land')


# 引数を受け取れるようにしたmulti_tello.py内multi_tello_command関数にcommandを渡す
import multi_tello
multi_tello.multi_tello_command(command)

```

### 実行

<img width="700" alt="tello-flight" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/da0804cf-52cc-5942-c0d9-17839c15fa2a.gif">
$\huge{・・・ヨシ！}$

よくない。いやー厳しい。
上記の通り、telloは斜め移動（というかマルチスレッド実行？）が出来ないので回転してから各方向に移動、という微妙な感じになる。目つむっておいて1ループ（回転＋xyz移動）ごとに目を開けばまぁ…
　
#展望編
一応プリミティブな軌道に沿ってTelloを飛ばす見通しが立ったは良いが、これ別にHoudiniを経由する必要がないやんけ・・・（再）
序盤に紹介したTwitterリンク先のドローンアートみたいなのを表現してみたい。。

### grid cube的なもの
というわけでお試しで組んでみる。
<img width="700" alt="grid-cube" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/f08ace33-b3bc-ea57-c778-ed4dbc146161.png">
<img width="700" alt="grid-cube-gif" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/152044/0835bc42-6f7e-26ae-2865-4459d8576a26.gif">

おお、ドローン編隊っぽい。
Houdiniを絡めるからにはここまでやりたかったけど、諸々の制約でアイデアメモに留めることに。

### 【Tello | Tello EDU】で編隊飛行
「だいぶ書き進めてから気づく」然りこれ然りなのだが、実はpython3に対応していてかつTelloもTello EDUも併せて編隊飛行できるリポジトリ（[dwalker-uk/TelloEduSwarmSearch(*GitHub)](https://github.com/dwalker-uk/TelloEduSwarmSearch)）があった…

斜め移動できない問題については[ここ(*Issue)](https://github.com/dwalker-uk/TelloEduSwarmSearch/issues/1)で触れられている。
アルゴリズムは使いまわせそうだから、この辺はMavic Mini辺りのSDK公開待ち、かな。。


　
以上、Houdiniからトイドローンを動かすというよりか、Houdini経由で「トイドローンを動かすpythonプログラム」を叩く記事でした。
プログラミング歴浅いので、いやそもそもここってこうでしょ？みたいな箇所があれば是非ご教授頂けると嬉しいです。