ミニドローン＋Tango＋Raspberry PiでドローンMRを実現する

天啓を得たのは、そう、5月の連休を明けた頃だった。
<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">さあ寝よ！というタイミングで「TangoでMambo」というフレーズが降臨しなすった(　ﾟдﾟ) <a href="https://t.co/S69Bo2FgIb">pic.twitter.com/S69Bo2FgIb</a></p>&mdash; jyuko (@jyuko49) <a href="https://twitter.com/jyuko49/status/861975897619836928">2017年5月9日</a></blockquote>
<script async src="//platform.twitter.com/widgets.js" charset="utf-8"></script>

思い付いてしまった以上、試さずにいられないのが、エンジニアの性というやつです。

## 目的
[Tango](https://get.google.com/tango/)はGoogleが開発したARプラットフォームで、深度カメラを搭載した対応スマートフォンをかざすことで、**物体までの距離を検知**することができます。空間検知の機能により、仮想のオブジェクトが現実の壁や床にぶつかって跳ね返るなど、**バーチャルとリアルが相互作用するコンテンツ**を作成することができます。

一方、[Mambo](https://www.parrot.com/jp/minidoron/parrot-mambo)はParrot製のミニドローンで、開発者用SDKが公開されているため、**自作のアプリケーションから制御**ができます。
ただし、GPSや動画用のカメラを持っておらず、SDKから位置や姿勢に関するデータを取得することが不可能なため、自律飛行や衝突回避といった高度な制御の実現は難しいです。

両者を組み合わせることで何ができそうかというと、

* **仮想空間でのイベントが現実世界のドローンに作用する（ドローン Mixed Reality）**

です。

**「TangoでMambo」は、意外と相性良いのでは？(｀・∀・´)**

ということで、プロトタイプ開発にて実現性を評価してみます。


## システム構成
### ハードウェア
- Parrot Mambo
- Lenovo Phab2 Pro
- Raspberry Pi3 Model B

### ソフトウェア
- Unity 5.6
- Tango SDK for Unity (Version 1.53, Hopak)
- Node.js v6.9.4
- Express v4.15.0
- noble v1.8.1
- rolling-spider v1.7.0

### 3Dモデル
- クエリちゃん SD版モデル

[![02_クエリちゃんライセンスロゴ-e1472646888241-300x256.png](https://qiita-image-store.s3.amazonaws.com/0/153460/148e3add-4811-9968-6aa2-b7facc5cc4e4.png)](http://query-chan.com/download/)

## 開発記録

### ドローンの位置を検知する
ドローンと仮想コンテンツの相互作用を起こすには、Tangoアプリ上でドローンの位置を捕捉する必要があります。

実現手法の一つとして考えられるのは、顔検知のように画像からドローンが映っている位置を特定し、その画素値に相当する方向に対して、物体までの距離をTangoから取得する方法です。

**なんとなく行けそう！だけど、理論に実装が追いつきません・・・（−＿−；）**

また、画像上にMamboが2体存在した場合に判別が必要になりそうです。
という訳で、今回はマーカー検知を使います。

Marker Ditectionは、Tango SDKのVersion 1.53, Hopakで追加されたAPIです。
APIにカメラ画像(ImageBuffer)を渡すと、画像内のARマーカーを検知し、マーカーの3次元上の位置、向きを教えてくれます。

```csharp
using System;
using System.Collections;
using System.Collections.Generic;
using Tango;
using UnityEngine;

public class MarkerDetectionUIController : MonoBehaviour, ITangoVideoOverlay
{
    public GameObject[] m_prefabs;
    private const double MARKER_SIZE = 0.1397;
    private GameObject[] m_objects;
    private List<TangoSupport.Marker> m_markerList;
    private TangoApplication m_tangoApplication;

    public void Start() {
        m_tangoApplication = FindObjectOfType<TangoApplication>();
        if (m_tangoApplication != null) {
            m_tangoApplication.Register(this);
        }

        m_objects = new GameObject[m_prefabs.Length];
        m_markerList = new List<TangoSupport.Marker>();
    }

    public void OnTangoImageAvailableEventHandler(TangoEnums.TangoCameraId cameraId,
        TangoUnityImageData imageBuffer) {
        TangoSupport.DetectMarkers(imageBuffer, cameraId,
            TangoSupport.MarkerType.ARTAG, MARKER_SIZE, m_markerList);

        for (int i = 0; i < m_markerList.Count; ++i) {
            TangoSupport.Marker marker = m_markerList[i];
            int markerId = Convert.ToInt32(marker.m_content);
            int objectIndex = (markerId - 1) % m_objects.Length;

			if (m_objects[objectIndex] == null) {
				m_objects[objectIndex] = Instantiate<GameObject>(m_prefabs[objectIndex]);
            }

			m_objects[objectIndex].transform.position = marker.m_translation;
			m_objects[objectIndex].transform.rotation = marker.m_orientation;
        }
    }
}
```
実装例は、Tango SDKのExamplesとほぼ同じです。GameObjectはマーカーに垂直の方向が上になるように配置されるので、デモではカメラの方を向くようにしています。


<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">Tangoのマーカー検知のテストです。<a href="https://twitter.com/hashtag/%E3%82%AF%E3%82%A8%E3%83%AA%E3%81%A1%E3%82%83%E3%82%93?src=hash">#クエリちゃん</a> <a href="https://twitter.com/hashtag/%E3%83%89%E3%83%AD%E3%83%BC%E3%83%B3?src=hash">#ドローン</a> <a href="https://t.co/i4aWvtcL3V">pic.twitter.com/i4aWvtcL3V</a></p>&mdash; jyuko (@jyuko49) <a href="https://twitter.com/jyuko49/status/863720813291122688">2017年5月14日</a></blockquote>
<script async src="//platform.twitter.com/widgets.js" charset="utf-8"></script>

移動中はマーカーが検知できていないですが、停止するとすぐに追従し、ホバリング中の小さな揺れでも、検知が途切れることはありません。
今のままでは、横や後ろを向いてしまうとマーカーが検知できない制限がありますが、ひとまずドローンの位置を特定できるようになりました。


### ドローン制御用APIサーバを構築する
Tangoアプリケーションを開発する場合、Tango SDK for Unityを使って、Unityで開発・ビルドするのが一般的です。(たぶん)

Mamboの制御に関しては、Android SDKが無償提供されているので、Unityに組み込むことができれば、アプリから直接ドローンを操作できるのですが、難易度はやや高めです。

そこで今回は、**ラズパイにNode.js＋ExpressでWeb APIサーバを立てて、ラズパイ経由でドローンに指令を送る方式**を採用しました。
メリットとして、実現性が見えている点が一つ。もう一つは、APIサーバがBluetoothのペアリングを行うことで、APIを介して複数のデバイスからドローンに関与できる点です。

[ラズパイ（Node.js）でのドローン制御は以前に実装済み](http://qiita.com/jyuko/items/cd39d93146fd392c8be3)なので、ExpressでAPIにしてあげます。
Expressのインストールは以下の記事を参考にさせて頂きました。for Windowsとなっていますが、Expressのインストール以降は同じ方法で大丈夫です。
[Node.js + ExpressでREST API開発を体験しよう for Windows［準備編］](http://qiita.com/tamura_CD/items/98bbd4a3143da960faf5)

```shell-session
$ npm install -g express express-generator
$ express mambo-web-server
$ cd mambo-web-server && npm install
$ npm install --save rolling-spider noble
```

上記のexpressコマンドでAPIサーバの雛形はできているので、ドローンを操作するインターフェースを追加します。

まず、app.jsに以下の2行を追加します。

```app.js
var mambo = require('./routes/mambo');
```

```app.js
app.use('/mambo', mambo);
```

続いて、routesディレクトリ配下にmambo.jsを作成し、以下のコードを書きます。

```mambo.js
var express = require('express');
var router = express.Router();

var rollingSpider = require('rolling-spider');
var drone = new rollingSpider({uuid:"[ドローンのUUID]"});

drone.connect(function () {
  drone.setup(function () {
    console.log('Configured for Mambo! ', drone.name);
    drone.flatTrim();
    drone.startPing();
    drone.flatTrim();
    setTimeout(function () {
      console.log(drone.name + ' => SESSION START');
    }, 1000);
  });
});

router.get('/takeoff', function(req, res, next){
  drone.takeOff();
  var response = {"result": "success"};
  res.header('Content-Type', 'application/json; charset=utf-8');
  res.send(response);
});

router.get('/landing', function(req, res, next){
  drone.land();
  var response = {"result": "success"};
  res.header('Content-Type', 'application/json; charset=utf-8');
  res.send(response);
});

module.exports = router;

```
[rolling-spider](https://github.com/voodootikigod/node-rolling-spider)はミニドローンを操作するオープンソースになります。
Node.jsのプロセス起動時にドローンとのペアリングが開始されるようにし、Webリクエストの受信時には、リクエストURLに応じたドローンの操作を実行、操作成功のレスポンスを返却となります。

ファイルを保存したら`npm start`でプロセスを立ち上げます。
nobleでBluetoothを利用するため、rootで実行もしくはsudoが必要です。

```shell-session
$ sudo npm start

> mambo-web-server@0.0.0 start /home/pi/work/mambo-web-server
> node ./bin/www

Configured for Mambo!  Mambo_XXXXXX
Mambo_462784 => SESSION START
```
最初の2行がプロセス起動時のログ、後の2行がドローンとペアリングした際に出力されるログです。
この状態でドローンの操作を試します。Web APIなので、ラズパイ内のブラウザでURLを叩けばテストできます。

http://localhost:3000/mambo/takeoff
→離陸

http://localhost:3000/mambo/landing
→着陸

難なく操作できました。

**やっぱ、自分のフィールド(Web)は落ち着くわ。(´ω｀)**

グローバルIPを紐付ければ、外出先から自宅のドローンをリモートで操作できそうですね。


### TangoアプリからAPI経由でドローンを操作する

インターフェースをWeb APIにしたのでUnityWebRequestでリクエストを投げることができます。

```csharp
using UnityEngine.Networking;

public class DroneController : MonoBehaviour {
	void Start () {
		takeOff ();
	}

	public void takeOff() {
		UnityWebRequest request = UnityWebRequest.Get ("http://192.168.0.4:3000/mambo/takeoff");
		request.Send ();
	}
}
```
簡単ですね。
接続先のIP（192.168.0.4）は、ラズパイに割り当てられているIPアドレスをifconfig等で調べて設定してください。


### ARコンテンツとドローンを相互作用させる

簡単な例で実装してみます。

* マーカーを検知したらGameObjectを配置し、ドローンを離陸させる
* アプリ上の仮想空間でボールを投げる
* ボールがGameObjectに当たった回数をカウントし、一定回数を超えたらドローンを着陸させる

現実のドローンの動きが仮想空間上のGameObjectに反映され、仮想空間でボールを当てるイベントがドローンの動きに反映されるため、簡単ではありますが、**リアルとバーチャルが相互作用するコンテンツ＝Mixed Reality**と言えるかと思います。

実装例は以下です。

```csharp
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Networking;

public class DroneController : MonoBehaviour {

	private int hitCount = 0;
	private QuerySDEmotionalController emotionalController;

	void Start () {
		emotionalController = FindObjectOfType<QuerySDEmotionalController> ();
		takeOff ();
	}
	
	void Update () {	
	}

	void OnCollisionEnter(Collision collision) {
		AndroidHelper.ShowAndroidToastMessage ("hit");
		hitCount += 1;

		switch (hitCount) {
		case 1:
			emotionalController.ChangeEmotion (QuerySDEmotionalController.QueryChanSDEmotionalType.NORMAL_ANGER);
			break;
		case 2:
			emotionalController.ChangeEmotion (QuerySDEmotionalController.QueryChanSDEmotionalType.NORMAL_GURUGURU);
			break;
		case 3:
			landing ();
			hitCount = 0;
			break;
		}
	}

	public void takeOff() {
		UnityWebRequest request = UnityWebRequest.Get ("http://192.168.0.4:3000/mambo/takeoff");
		request.Send ();
	}

	public void landing() {
		UnityWebRequest request = UnityWebRequest.Get ("http://192.168.0.4:3000/mambo/landing");
		request.Send ();
	}
}
```
マーカーに追従させるクエリちゃんモデルには、Colliderがセットしてあり、ボールが当たるとOnCollisionEnterイベントが発生します。2回まではクエリちゃんの表情を変えるだけですが、3回目にlandingのリクエストを送り、ドローンを着陸させることで、ボールが当って落ちたように見せます。
ボールを投げる処理は、ExperimentalMeshBuilderWithPhysicsのBallManagerを使っているので、説明は割愛します。


<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">バーチャルなボールが一定回数hitしたら、リアルのミニドローンが落ちるデモ。<br>当たり判定用のGameObjectはマーカー検知で配置。いずれはドローン自体を画像から検知したい。<a href="https://twitter.com/hashtag/Tango?src=hash">#Tango</a> <a href="https://twitter.com/hashtag/%E3%82%AF%E3%82%A8%E3%83%AA%E3%81%A1%E3%82%83%E3%82%93?src=hash">#クエリちゃん</a> <a href="https://twitter.com/hashtag/%E3%83%89%E3%83%AD%E3%83%BC%E3%83%B3?src=hash">#ドローン</a> <a href="https://t.co/bzvIjUOc8l">pic.twitter.com/bzvIjUOc8l</a></p>&mdash; jyuko (@jyuko49) <a href="https://twitter.com/jyuko49/status/864611839853772800">2017年5月16日</a></blockquote>
<script async src="//platform.twitter.com/widgets.js" charset="utf-8"></script>

HTTP通信でラズパイを経由する分の遅延が気になるかもしれませんが、インターネットに出る訳ではないので、反応速度はかなり速いです。


## まとめ
Tangoを用いて、仮想空間からドローンに作用するドローンMRを実現しました。アプリからドローンを操作する方法として、ラズパイでAPIサーバを構築しています。

長くなったので、この辺りで記事を分けますが、途中で触れた通り、ドローンの位置を捕捉する方法は改善の余地がありますし、複数デバイスでの関与（ボール投げて当てる側とドローンを操作して避ける側でプレイ）も簡単に実現できると思います。
また、今回のデモではTangoの空間検知をあまり活用できていません。実空間の障害物を検知して避けて飛行する指示を送ったり、実空間の形状に合わせて仮想の障害物を置くなど、より仮想と現実がMixしたドローンMRの開発を継続していく予定です。