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

天啓を得たのは、そう、5月の連休を明けた頃だった。

さあ寝よ！というタイミングで「TangoでMambo」というフレーズが降臨しなすった(　ﾟдﾟ) pic.twitter.com/S69Bo2FgIb

— jyuko (@jyuko49) 2017年5月9日

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

目的

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

一方、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版モデル

開発記録

ドローンの位置を検知する

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

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

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

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

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

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はマーカーに垂直の方向が上になるように配置されるので、デモではカメラの方を向くようにしています。

Tangoのマーカー検知のテストです。#クエリちゃん #ドローン pic.twitter.com/i4aWvtcL3V

— jyuko (@jyuko49) 2017年5月14日

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

ドローン制御用APIサーバを構築する

Tangoアプリケーションを開発する場合、Tango SDK for Unityを使って、Unityで開発・ビルドするのが一般的です。(たぶん)

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

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

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

$ 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はミニドローンを操作するオープンソースになります。
Node.jsのプロセス起動時にドローンとのペアリングが開始されるようにし、Webリクエストの受信時には、リクエストURLに応じたドローンの操作を実行、操作成功のレスポンスを返却となります。

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

$ 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でリクエストを投げることができます。

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と言えるかと思います。

実装例は以下です。

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を使っているので、説明は割愛します。

バーチャルなボールが一定回数hitしたら、リアルのミニドローンが落ちるデモ。
当たり判定用のGameObjectはマーカー検知で配置。いずれはドローン自体を画像から検知したい。#Tango #クエリちゃん #ドローン pic.twitter.com/bzvIjUOc8l

— jyuko (@jyuko49) 2017年5月16日

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

まとめ

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

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