AR.Drone for AS3 と RTMFP と AIR for iOS で Drone を飛ばしてみました

最近ちょっと仕事が忙しくなってきたことと、AR.Drone からの映像の取得でつまづいて開発が滞っている AR.Drone for AS3 ですが、先日「RTMFP 野郎の会」が開催されて以降ブーム（？）の RTMFP を使って、モバイルデバイス（iPhone, iPad）で操作する実験をしてみました。

まぁもともとこの Drone のネイティブの開発環境では iOS を使用したモバイルデバイスで飛ばすことは普通なのですが、今回は AS3 で飛ばすというプロジェクトなので、Drone とのデータの送受信に不可欠な flash.net.DatagramSocket がモバイルデバイスでは使えないため…
デスクトップアプリを中間サーバとしてシステムを作る必要がありました。

で、今回の動作フローとしては…

1. AR.Drone に内蔵されているルータに Mac と iPad を WiFi で接続。

2. Mac 上の AIR アプリと iPad 上の AIR アプリを RTMFP で接続。

3. iPad のアプリから RTMFP 経由で送信された操作コマンドを Mac の中間サーバアプリで受け取り、DatagramSocket（UDP ソケット）経由で Drone に送信。

4. Drone から送信されてくるナビゲーションデータは DatagramSocket 経由で Mac で受信し、データをパースしたあと RTMFP 経由で iPad に送信。

とまぁ、簡単に書くとこのような流れで動いています。
飛んでる様子はこんな感じ。（部屋が狭くて墜落したりしていますが…）

　
では、今回使用したコードの抜粋です。
AR.Drone との通信につきましては以前の記事に書いていますので割愛して、今回は RTMFP の部分のコードを抜粋します。

RTMFP で通信するため Mac 側と iPad 側両方に NetConnection を作って “rtmfp:” で接続します。

public function doConnect():void 
{
	_nc = new NetConnection();
	_nc.addEventListener(NetStatusEvent.NET_STATUS, netStatusHandler);
	_nc.connect("rtmfp:");
}

次に両方を同じ NetGroup に登録します。

public function addNetGroup():void
{
	_gs = new GroupSpecifier(GROUP_NAME);
	_gs.postingEnabled = true;
	_gs.routingEnabled = true;
	_gs.ipMulticastMemberUpdatesEnabled = true;
	_gs.multicastEnabled = true;
	_gs.addIPMulticastAddress("224.0.0.1:30303"); // local 224.0.0.0 ~ 224.0.0.255 // grovbal 224.0.1.0 ~ 238.255.255.255
		
	// NetGroup
	_ng = new NetGroup(_nc, _gs.groupspecWithAuthorizations());
	_ng.addEventListener(NetStatusEvent.NET_STATUS, netStatusHandler);
}

これで RTMFP の接続は完了です。

無事接続ができたら Drone を飛ばすためのコマンドを送信します。
iPad アプリから Mac のアプリへコマンドを送信するための送信用関数を定義しておきます。

public function sendMsg(id:int, str:String = ""):void
{
	var message:Object = new Object();
	message.id = id;
	message.text = str;
	message.nowDate = new Date();
	message.sender = _ng.convertPeerIDToGroupAddress(_nc.nearID); 
	_ng.sendToNeighbor(message, NetGroupSendMode.NEXT_DECREASING);
}

こんな感じで上記の送信用関数を引数でコマンドを付けて呼びます。

private function onTakeoffAndLanding( e:MouseEvent ):void
{
	if( _isLanding )
	{
		_rtmfp.sendMsg( 1, "takeoff" );
		_isLanding = false;
	}
	else
	{
		_rtmfp.sendMsg( 1, "landing" );
		_isLanding = true;
	}
}
		
// Left Controller ( Roll, Pitch Control )
private function onMoveCtrl( e:MouseEvent ):void
{
	addEventListener( Event.ENTER_FRAME, onMoveCtrlUpdate );
	mainUI.vStick_A.ctrlPoint.startDrag( false, _ctrlRect );
}

private function onMoveCtrlUp( e:MouseEvent ):void
{
	removeEventListener( Event.ENTER_FRAME, onMoveCtrlUpdate );
	_rtmfp.sendMsg( 4, "stop" );
	mainUI.vStick_A.ctrlPoint.stopDrag();
	mainUI.vStick_A.ctrlPoint.x = mainUI.vStick_A.ctrlPoint.y = 0;
}
		
private function onMoveCtrlUpdate( e:Event ):void
{
	var sendData:String = mainUI.vStick_A.ctrlPoint.x + "," + mainUI.vStick_A.ctrlPoint.y;
	_rtmfp.sendMsg( 2, sendData );
}
		
// Right Controller ( Yaw, Altitude Control )
private function onVerticalAndTurnCtrl( e:MouseEvent ):void
{
	addEventListener( Event.ENTER_FRAME, onVerticalAndTurnCtrlUpdate );
	mainUI.vStick_B.ctrlPoint.startDrag( false, _ctrlRect );
}

private function onVerticalAndTurnCtrlUp( e:MouseEvent ):void
{
	removeEventListener( Event.ENTER_FRAME, onVerticalAndTurnCtrlUpdate );
	_rtmfp.sendMsg( 4, "stop" );
	mainUI.vStick_B.ctrlPoint.stopDrag();
	mainUI.vStick_B.ctrlPoint.x = mainUI.vStick_B.ctrlPoint.y = 0;
}

iPad から RTMFP 経由で送られてきたデータを Mac 側で受信すると、受信したコマンドに応じて UDP ソケット経由で Drone にコマンドを送信します。

private function onMsgPosted(e:Event):void
{
	var id:int = int( _rtmfp.msgObj.id );
	var msg:String = _rtmfp.msgObj.text.toString();
	var ary:Array;
	
	// Takeoff or Landing
	if( id == 1 )
	{
		if( msg == "takeoff" )
		{
			_arDrone.takeoff();
			mainUI.vStick_A.visible = mainUI.vStick_B.visible = mainUI.drone.visible = mainUI.altimeter.visible = true;
			mainUI.bt_takeoff.gotoAndStop( 2 );
			mainUI.bg.alpha = .3;
		}
		else if( msg == "landing" )
		{
			_arDrone.landing();
			mainUI.vStick_A.visible = mainUI.vStick_B.visible = mainUI.drone.visible = mainUI.altimeter.visible = false;
			mainUI.bt_takeoff.gotoAndStop( 1 );
			mainUI.bg.alpha = 1;
		}
	}
	// Roll and Pitch
	else if( id == 2 )
	{
		ary = msg.split( "," );
		_arDrone.moveAllDirection( int(ary[0]) * .005, int(ary[1]) * .005 );
	}
	// Yaw or Vertical
	else if( id == 3 )
	{
		ary = msg.split( "," );
		
		var dx:int = int(ary[0]);
		var dy:int = int(ary[1]);
		
		if( ( dx < 0 ? -dx : dx ) > ( dy < 0 ? -dy : dy ) )
		{
			_arDrone.turn( dx );
			mainUI.drone.rotation += dx * .08;
		}
		else
		{
			_arDrone.upDown( dy * .5 );
		}
	}
	// Stop
	else if( id == 4 )
	{
		if( msg == "stop" )
		{
			_arDrone.moveStop();
		}
	}	
}

これで Drone を操作できるようになりました。

次に Drone から送信されてくるナビゲーションデータは、UDP ソケット経由で受信したバイナリをパースして得たデータをこのように文字列でくっつけて、RTMFP経由 で Mac から iPad に送信します。

var droneState:String = _arDrone.batteryPercentage + "," + _arDrone.roll + "," + _arDrone.pitch + "," + _arDrone.yaw + "," + _arDrone.altitude;
_rtmfp.sendMsg( 0, droneState );

このデータを受信したらパースしてパラメータを UI に反映させます。

private function onMsgPosted(e:Event):void
{
	var msg:String = _rtmfp.msgObj.text.toString();
	var ary:Array = msg.split( "," );
	
	// state
	mainUI.batMeter.tx.text = ary[0].toString();
	mainUI.batMeter.level.scaleX = ary[0] * .01;
	
	mainUI.drone.rotationX = ary[2] * 2;
	mainUI.drone.rotationY = ary[1] * 2;
	
	var altParcent:Number = ( ary[4] * .1 ) / 250;
	if(altParcent > 0) mainUI.altimeter.alitimatePoint.y = mainUI.altimeter.track.height - ( mainUI.altimeter.track.height *  altParcent);
}

こんな感じで今回 RTMFP を使って 複数のデバイスを連動させて Drone を飛ばしてみましたが、この仕組みが使えると、例えば Drone を iPnone で操作して飛ばしながら PC 側の画面に Drone の飛行データをパラメータとしてグラフィックを描画する「AR.Drawing」や、同じく Drone の飛行データを利用して PC 側では 3D のマップ上を Drone が飛行するアニメーションを大画面のスクリーンに投影するなど…

ちょっと想像しただけでもなんかいろいろできそうで、これはこれで楽しいですよね。