ミニドローンを制御するLeap Motionコントローラを作る（ジェスチャー版）

久しぶりにミニドローンを飛ばしました。前回に引き続き、Leap Motionでミニドローンを操作するコントローラを作成しています。

先に断っておくと、フレーム差分から手の動きを検知してドローンと同期させる実装は、なかなかどうして難しく、まだ実現できていません。
そのため、ジャスチャーイベントによる制御を先に実装したいと思います。

システム構成

前回の記事から変更していませんので、そちらを参照ください。
ミニドローンをRaspberry Piと接続してLeap Motionで飛ばす

開発記録

ネットワーク構成を見直す

MacBookとラズパイのWeb Socket通信をWiFiテザリングで行っていたのですが、MacBookとラズパイを同じWiFiルータに接続し、プライベートIPで接続することで有線LANが不要になりました。

Leap Motionの設定を見直す

前回のデモ動画では、何度かジェスチャーに失敗した後、急にミニドローンが飛び立つような動きになっていました。
その後、開発を続ける中で、ジェスチャーの失敗だけではなく、成功時にもラグが発生していたことが判明しました。

原因として、frameEventNameの設定を'animationFrame'(60fps)にするとラズパイの処理が追い付かないため、'deviceFrame'(処理能力に応じたfps)にしていたのですが、この設定でも大きなラグが出ることがあります。
試しに、"いずれも指定しない"という定義を試したところ、反応速度が改善しました。（このあたりの詳しい理由は、まだわかっていません）

var controller = new Leap.Controller({
    host: '192.168.0.4',
    port: 6437,
    //ラズパイでは'animationFrame'も'deviceFrame'も指定しない
    //frameEventName: 'animationFrame',
    enableGestures: true
});

実際のfpsは、Frame.currentFrameRateで出力でき、25-40fpsとなっていました。

ジャスチャーイベントの割り当てを考える

Leap Motionで検知できる基本のジェスチャーは4つあります。

Gesture.type	アクション	操作の難易度
CircleGesture	指で円を描く	簡単
SwipeGesture	指を平行移動する	簡単
ScreenTapGesture	指で前方をタップする	難しい
KeyTapGesture	指で下方向にタップする	難しい

CircleとSwipeは操作が簡単ですが、その分、操作したつもりがないのに誤って検知されてしまうケースがままあります。
また、KeyTapはジェスチャーが大きくなると、下方向のSwipeとして検知されることがあります。

上記を踏まえて、以下の制御を割り当てました。

離陸　→　KeyTapGesture
移動(6方向)　→　SwipeGesture
宙返り(4方向)　→　CircleGesture
着陸　→　ScreenTapGesture

最も誤検知が起きにくいScreenTapGestureを着陸にすることで、意図せずフライトが中断してしまう事態を避けます。
KeyTapと下方向のSwipeは着陸中と離陸中の操作になるので、一方の操作が有効なとき、もう一方の操作を誤って検知しても何も起きません。
旋回（回転して向きを変える）にはジェスチャーを割り当てていないので、現状は平行移動のみとなります。

ちなみに、公式のアプリではカメラ撮影やキャノンの発射（ロボットアームの操作）ができるのですが、node-rolling-spiderにそれらしきメソッドは見当たりません。
対応するには、BlueToothでドローンに信号を送るところから開発が必要になるかもしれないです。

CircleGestureを設定する

CircleGesture.normal（描いた円の法線ベクトル）を使って、回転の向きを判別します。

前提として、Leap Motionは右手座標系です。
また、Vectorは、[0]:x座標、[1]:y座標、[2]:z座標の配列となります。

正面を向いて時計回りに円を描いたとします。normalは正面(z軸マイナス方向)に向かって伸びます。反時計回りに円を描いた場合、normalは手前(z軸プラス方向)になります。
今度は、手前から前方に縦向きの円を描きます。時計回りに描くと、normalは左(x軸マイナス方向)に向き、反時計回りに描くとnormalは右(x軸プラス方向)を向きます。

上記から、normal[0](=x座標)とnormal[2](=z座標)の絶対値を比較し、回転が縦方向か横方向かを判別します。
絶対値が大きい方の座標について、正負で前後と左右が判別できるため、縦横×正負で4方向の回転パターンにドローンの制御を紐付けます。

function onCircle(gesture){
    if (gesture.state === 'stop') {
        if (Math.abs(gesture.normal[0]) > Math.abs(gesture.normal[2])){
            if (gesture.normal[0] > 0) {
                d.backFlip({ steps: STEPS });
                console.log('circle => backFlip');
            } else {
                d.frontFlip({ steps: STEPS });
                console.log('circle => frontFlip');
            }
        } else {
            if (gesture.normal[2] > 0) {
                d.leftFlip({ steps: STEPS });
                console.log('circle => leftFlip');
            } else {
                d.rightFlip({ steps: STEPS });
                console.log('circle => rightFlip');
            }
        }
    }
}

SwipeGestureを実装する

SwipeGesture.directionを使って、Swipeの方向を判定します。

方向判別の考え方は、CircleGestureと同じです。
SwipeGestureにnormalプロパティはなく、directionが指の動いた方向になります。CircleGestureで扱った4方向に加え、上下(y軸)の移動も判別します。

function onSwipe(gesture){
    if (gesture.state === 'stop') {
        var direction, swipeDirection;

        if(Math.abs(gesture.direction[0]) > Math.abs(gesture.direction[1])){
            if(Math.abs(gesture.direction[0]) > Math.abs(gesture.direction[2])){
                direction = 'x';
            } else {
                direction = 'z';
            }
        } else {
            if(Math.abs(gesture.direction[1]) > Math.abs(gesture.direction[2])){
                direction = 'y';
            } else {
                direction = 'z';
            }
        }

        if (direction === 'x') {
            if(gesture.direction[0] > 0){
                swipeDirection = "right";
                d.tiltRight({ steps: STEPS });
            } else {
                swipeDirection = "left";
                d.tiltLeft({ steps: STEPS });
            }
        } else if (direction === 'y') {
            if(gesture.direction[1] > 0){
                swipeDirection = "up";
                d.up({ steps: STEPS });
            } else {
                swipeDirection = "down";
                d.down({ steps: STEPS });
            }
        } else if (direction === 'z'){
            if(gesture.direction[2] > 0){
                swipeDirection = "back";
                d.backward({ steps: STEPS });
            } else {
                swipeDirection = "front";
                d.forward({ steps: STEPS });
            }
        }
        console.log('swipe => '+swipeDirection);
    }
}

ジェスチャーイベントを完成させる

未実装のジェスチャーはKeyTapGesture、ScreenTapGestureになりますが、これらは操作の方向が固定されており、検知してドローンの制御と紐付けるだけです。
ただし、ドローンとの接続前にtakeOffが発生するとNode.jsのプロセスが異常終了してしまうため、注意が必要です。

leapController.js

var RollingSpider = require('rolling-spider');
var Leap = require('leapjs');

var ACTIVE = false;
var STEPS = 5;
var d = new RollingSpider({uuid:"xxxxxxxxxxxxxxxx"});

d.connect(function () {
    d.setup(function () {
        console.log('Configured for Rolling Spider! ', d.name);
        d.flatTrim();
        d.startPing();
        d.flatTrim();
        setTimeout(function () {
            console.log(d.name + ' => SESSION START');
            ACTIVE = true;
        }, 1000);
    });
});

var controller = new Leap.Controller({
    host: '192.168.0.4',
    port: 6437,
    //ラズパイでは'animationFrame'も'deviceFrame'も指定しない
    //frameEventName: 'animationFrame',
    enableGestures: true
});

controller.on('gesture', function (gesture) {
    switch (gesture.type) {
        case 'circle':
            onCircle(gesture);
            break;
        case 'swipe':
            onSwipe(gesture);
            break;
        case 'screenTap':
            onScreenTap(gesture);
            break;
        case 'keyTap':
            onKeyTap(gesture);
            break;
    }
});

function onCircle(gesture){
    if (gesture.state === 'stop') {
        if (Math.abs(gesture.normal[0]) > Math.abs(gesture.normal[2])){
            if (gesture.normal[0] > 0) {
                d.backFlip({ steps: STEPS });
                console.log('circle => backFlip');
            } else {
                d.frontFlip({ steps: STEPS });
                console.log('circle => frontFlip');
            }
        } else {
            if (gesture.normal[2] > 0) {
                d.leftFlip({ steps: STEPS });
                console.log('circle => leftFlip');
            } else {
                d.rightFlip({ steps: STEPS });
                console.log('circle => rightFlip');
            }
        }
    }
}

function onSwipe(gesture){
    if (gesture.state === 'stop') {
        var direction, swipeDirection;

        if(Math.abs(gesture.direction[0]) > Math.abs(gesture.direction[1])){
            if(Math.abs(gesture.direction[0]) > Math.abs(gesture.direction[2])){
                direction = 'x';
            } else {
                direction = 'z';
            }
        } else {
            if(Math.abs(gesture.direction[1]) > Math.abs(gesture.direction[2])){
                direction = 'y';
            } else {
                direction = 'z';
            }
        }

        if (direction === 'x') {
            if(gesture.direction[0] > 0){
                swipeDirection = "right";
                d.tiltRight({ steps: STEPS });
            } else {
                swipeDirection = "left";
                d.tiltLeft({ steps: STEPS });
            }
        } else if (direction === 'y') {
            if(gesture.direction[1] > 0){
                swipeDirection = "up";
                d.up({ steps: STEPS });
            } else {
                swipeDirection = "down";
                d.down({ steps: STEPS });
            }
        } else if (direction === 'z'){
            if(gesture.direction[2] > 0){
                swipeDirection = "back";
                d.backward({ steps: STEPS });
            } else {
                swipeDirection = "front";
                d.forward({ steps: STEPS });
            }
        }
        console.log('swipe => '+swipeDirection);
    }
}

function onScreenTap(gesture){
    if (gesture.state === 'stop') {
        console.log('screenTap => landing');
        d.land();
    }
}

function onKeyTap(gesture){
    if (gesture.state === 'stop') {
        if (ACTIVE) {
            console.log('keyTap => takeOff');
            d.takeOff();
        }
    }
}

ひとまず、ジェスチャーイベントは完成しました。（╹◡╹）

デモ飛行してみる

ミニドローン制御の改良版。いつもより多めに回しております。 pic.twitter.com/UHVAnSYpSR

— jyuko (@jyuko49) 2017年2月21日

Leap Motionの設定を見直した効果が出ており、ジェスチャーを行った直後にドローンが反応するため、違和感のない操作ができます。

一方、新たに見つかった課題として、

ドローンを見ていると、手元が全然見えない！(・∀・)

割とジェスチャーに失敗します。

まとめ

というか今回のオチ。

今回の件から得るべき教訓は、「試す前に気付けよ！」と思うようなことでも、案外、実際に試してみないとわからないということだ。

とは言え、前回よりは明らかに操作しやすくなっている訳で、着実に前進はしています。
改善策として、ヘッドバンドでLeap Motionをおでこに固定する方法を閃いたのですが、それはそれでビジュアル的な問題をクリアしなければなりません。

なお、今回見送ったフレームイベントによる制御ですが、前後、上下、左右の3方向の動きが同時に行われるため、処理が追い付かず、空中でフラついて衝突をくり返しています。
fpsを落とすなどリアルタイム性を犠牲にするか、根本的な見直しを行わないと難しそうな雰囲気です。