CV Drone 日本語版 公開

※2017/12/01 リンク切れ修正
お待たせしました。

AR.DroneをC++で動かすライブラリ「CV Drone」日本語版を公開します。
※CV Drone (= OpenCV + AR.Drone) はAR.Droneを操作するための「非公式な」ライブラリです。


CV Drone 日本語版のダウンロード - 34.1MB
最新版(英語版)のダウンロードはこちらへどうぞ。
※引用文献の記述は"puku0x, CV Drone (= OpenCV + AR.Drone), https://github.com/puku0x/cvdrone (アクセス日)"でOKです

特徴は以下の通りです。
・必要なライブラリが最初から全部入っている
・AR.Drone 1.0/2.0両対応
・Visual C++ 2008/2010/2012/2013対応
・ARDroneForP5ライクなインターフェース
・IplImage/cv::Mat形式での画像の取得が可能
・マルチスレッド化されているため処理が高速

「とにかく簡単に導入できるように」を開発の方針としています。
面倒なライブラリのインストールや、パスの設定は必要ありません。

ダウンロードしたら好きな場所に解凍して、



お使いのVisualStudioのバージョン用のソリューションファイルを開き、



「Release」モードでビルド。これだけ！



ちなみに画像を表示するだけなら30行ぐらいあれば出来ます。

#include "ardrone/ardrone.h"

// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // ARDrone
    ARDrone ardrone("192.168.1.1");

    // メインループ
    while (1) {
        // 更新
        if (!ardrone.update()) break;

        // 画像の取得
        IplImage *image = ardrone.getImage();

        // 表示
        cvShowImage("camera", image);

        // Escで終了
        if (cvWaitKey(1) == 0x1b) break;
    }

    return 0;
}

動作確認は以下の環境で行っています。
・Windows 7
・VC++ 2005/2008/2010/2012
・AR.Drone 1.0 (ファームウェア Ver. 1.11.15)
・AR.Drone 2.0 (ファームウェア Ver. 2.4.1)

---

AR.Droneを用いたC++アプリケーションの開発は、公式のフォーラムを始め、工学ナビの中の人のブログや、AR.Droneを楽しむ会、最近のAR.Drone2.0に関してはAR.Drone Developmentといったサイトが参考になります。

AR.Droneを動かそう（H.264ビデオのデコード）

AR.Drone 2.0は動画をH.264形式で送ってきます。

※本記事中のH.264ビデオデコード方法はFFmpegのチュートリアル及び、AR.Drone Development様の記事を参考にしております。

やり方はこんな感じです。UVLCと比べて少し多くなります。
1. FFmpegの初期化
2. avformat_open_input()でビデオのポート(5555番)を開く
3. avformat_find_stream_info()でストリーム情報を取得
4. avcodec_find_decoder()で対応するデコーダを見つける
5. avcodec_open2()でコーデックを開く
6. ビデオ用のバッファを確保
7. 画像用のIplImageを確保
8. av_read_frame()でフレームを読み込む(内部的にメモリ確保されるので解放を忘れずに)
9. avcodec_decode_video2()でビデオをデコード
10. デコードできたらsws_scale()でBGRに変換
11. IplImageにコピー
12. cvShowImage()で表示

#include <stdio.h>
 
// OpenCV
#include <opencv2/opencv.hpp>

// FFmpeg
extern "C" {
  #include "libavcodec/avcodec.h"
  #include "libavformat/avformat.h"
  #include "libswscale/swscale.h"
}

// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // AR.Droneのアドレス
    char *drone_addr = "tcp://192.168.1.1:5555";

    // FFmpeg初期化
    av_register_all();
    avformat_network_init();

    // ビデオを開く
    AVFormatContext *pFormatCtx = NULL;
    if (avformat_open_input(&pFormatCtx, drone_addr, NULL, NULL) != 0) {
        printf("ERROR: avformat_open_input() failed. (%s, %d)\n", __FILE__, __LINE__);
        return -1;
    }

    // ストリーム情報の取得
    if (avformat_find_stream_info(pFormatCtx, NULL) < 0) return -1;
    av_dump_format(pFormatCtx, 0, drone_addr, 0);

    // コーデックの検索
    AVCodecContext *pCodecCtx = pFormatCtx->streams[0]->codec;
    AVCodec *pCodec = avcodec_find_decoder(pCodecCtx->codec_id);

    // コーデックを開く
    if (avcodec_open2(pCodecCtx, pCodec, NULL) < 0) {
        printf("ERROR: avcodec_open2() failed. (%s, %d)\n", __FILE__, __LINE__);
        return -1;
    }

    // ビデオ
    AVFrame *pFrame = avcodec_alloc_frame();
    AVFrame *pFrameBGR = avcodec_alloc_frame();

    // バッファの確保
    uchar *buffer = (uchar*)av_malloc(avpicture_get_size(PIX_FMT_BGR24, pCodecCtx->width, pCodecCtx->height));

    // バッファと画像を関連付ける
    avpicture_fill((AVPicture*)pFrameBGR, buffer, PIX_FMT_BGR24, pCodecCtx->width, pCodecCtx->height);

    // 変換コンテキスト
    SwsContext *pConvertCtx = sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt,  pCodecCtx->width, pCodecCtx->height, PIX_FMT_BGR24, SWS_SPLINE, NULL, NULL, NULL);

    // 画像用のメモリ確保
    IplImage *img = cvCreateImage(cvSize(pCodecCtx->width, pCodecCtx->height), IPL_DEPTH_8U, 3);

    // メインループ
    while (1) {
        AVPacket packet;
        int frameFinished;

        // フレーム読み込み
        while (av_read_frame(pFormatCtx, &packet) >= 0) {
            // デコード
            avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, &packet);

            // デコード完了
            if (frameFinished) {
                sws_scale(pConvertCtx, (const uchar* const*)pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameBGR->data, pFrameBGR->linesize);
                memcpy(img->imageData, pFrameBGR->data[0], pCodecCtx->width * pCodecCtx->height * sizeof(uchar) * 3);
            }

            // メモリ解放
            av_free_packet(&packet);
        }

        // 表示
        cvShowImage("img", img);
        if (cvWaitKey(1) == 0x1b) break;
    }

    // 画像用のメモリ解放
    cvReleaseImage(&img);

    // FFmpeg終了
    sws_freeContext(pConvertCtx);
    av_free(buffer);
    av_free(pFrameBGR);
    av_free(pFrame);
    avcodec_close(pCodecCtx);
    avformat_close_input(&pFormatCtx);
 
    return 0;
}

プログラムを実行すると、


このように表示されます。

海外フォーラムでもまだ「カメラの切り替え」や「720p出力」といったことについてはあまり触れられていないので、今後はそれらを紹介していこうかと思います。

気になる日本語版は現在準備中です。もう少しお待ちください。

AR.Droneを動かそう（UVLCビデオのデコード）

AR.Drone 1.0は動画をUVLC（Universal-VLC）形式で送ってきます。

海外のフォーラムでも積極的に情報交換が行われており、ARDrone-Control-.NETやJavadroneはその先駆けといえるでしょう。

UVLCの詳細はデベロッパーガイドに記載されていますが、素人の私には何が何やら…。

幸いにも、C++への実装は既にAR.Drone Development様をはじめ、MarcosさんやPongsakさんがやってくれています。当ブログでは彼らのソースを参考に 簡単に使えるように作り直したものを配布しています

さて、UVLCデコード→表示の手順は以下の通りです。
1. ビデオのポート(5555番)を開く
2. ビデオ用のバッファを確保
3. 画像用のIplImageを確保
4. ビデオのポートにパケットを送る
5. 何か受信したらデコード(UVLC::DecodeVideoというのがデコード関数です)
6. IplImageにコピー
7. cvShowImage()で表示

#include <stdio.h>
#include <stdlib.h>

// Win32API
#include <windows.h>

// OpenCV
#include <opencv2/opencv.hpp>

// UVLC
#include "uvlc.h"

#define ARDRONE_DEFAULT_ADDR     "192.168.1.1"   // ARDroneデフォルトIPアドレス
#define ARDRONE_VIDEO_PORT       (5555)          // ビデオ用ポート

#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)

// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // WSA初期化
    WSAData wsaData;
    WSAStartup(MAKEWORD(1,1), &wsaData);

    // ソケット
    UDPSocket sockVideo;

    // ビデオ用のポートを開く
    if (!sockVideo.open(ARDRONE_DEFAULT_ADDR, ARDRONE_VIDEO_PORT)) {
        printf("ERROR: UDPSocket::open(port=%d) failed. (%s, %d)\n", ARDRONE_VIDEO_PORT, __FILE__, __LINE__);
        return 0;
    }

    // バッファの確保
    int width = 320;
    int height = 240;
    uchar *bufferBGR = (uchar*)malloc(width * height * 3 * sizeof(uchar));

    // メインループ
    while (1) {
        // リクエストを送信
        sockVideo.sendf("\x01\x00\x00\x00");

        // 受信する
        uchar buf[122880];
        int size = sockVideo.receive((void*)&buf, sizeof(buf));

        // 何か受信した
        if (size > 0) {
            // ビデオをデコード
            UVLC::DecodeVideo(buf, size, bufferBGR, &width, &height);

            // 画像用のメモリ確保
            IplImage *img = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
            img->imageData = (char*)bufferBGR;

            // 表示
            cvShowImage("camera", img);

            // 画像用のメモリ解放
            cvReleaseImageHeader(&img);
        }

        // Escで終了
        if (cvWaitKey(30) == 0x1b) break;
    }

    // バッファの解放
    free(bufferBGR);

    // ポートを閉じる
    sockVideo.close();

    // WSA終了
    WSACleanup();

    return 0;
}

udp.h
udp.cpp
uvlc.h

取得した画像がこちら。



画像の解像度についてですが、現状のファームウェアでは320x240で固定されています。

これはCPUの性能の限界と、通信の品質を維持するためだそうです（公式フォーラムより）。

偉大な先人たちに感謝！ということでまた次回。

AR.Droneを動かそう（ナビゲーションデータの取得）

ナビゲーションデータはAR.Droneのポート5554番から取得できます。

しかし、単純にrecvfrom()とかしても何も入ってきません。

ちゃんと手順を踏んでみましょう。
1. ナビゲーションデータのポート(5554番)を開く
2. ATコマンドのポート(5556番)を開く
3. 何か送る
4. AT*CONFIGでデモ用データを送るように設定
5. AT*CTRLでACKを送信
6. ナビゲーションデータのポートにリクエストを送る
7. 何か受信したら構造体にコピー

udp.h
udp.cpp

データの取得がうまくいかないときはAR.Droneを再接続してみましょう。

また、ファームウェアのバージョンが古い場合、Boorstrapモードの解除に失敗することが多いようです。
SDK2.0に同梱されている最新のAR.Drone用ファームウェア(1.10.14と2.1.20)に更新しましょう。

次回は動画を取得します。

AR.Droneを動かそう（コマンドの送信）

AR.Droneの制御はATコマンドという特殊な文字列を送信することで行われています。

代表的なコマンドをいくつか挙げます。
・AT*REF
　離着陸や緊急停止といった司令を送ります。

・AT*PCMD
　速度司令を送ります。モード1は通常動作、モード0はホバリング動作です。

・AT*CONFIG
　カメラの切り替えやナビゲーションデータの設定に使われます。今回は出てきません。

それぞれシーケンス番号と謎の数列を入れて、UDPポート5556に送ればAR.Droneが動きます。

// UDP
#include "udp.h"

#define ARDRONE_DEFAULT_ADDR  "192.168.1.1"   // ARDroneデフォルトIPアドレス
#define ARDRONE_COMMAND_PORT  (5556)          // ATコマンド用ポート

#define KEY_DOWN(key) (GetAsyncKeyState(key) & 0x8000)
#define KEY_PUSH(key) (GetAsyncKeyState(key) & 0x0001)

// --------------------------------------------------------------------------
// main(引数の数、引数リスト)
// メイン関数です
// 戻り値 正常終了:0 エラー:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
    // WSA初期化
    WSAData wsaData;
    WSAStartup(MAKEWORD(1,1), &wsaData);

    // シーケンス番号
    int seq = 1;

    // UDPソケット
    UDPSocket sockCommand;

    // コマンド用のポートを開く
    if (!sockCommand.open(ARDRONE_DEFAULT_ADDR, ARDRONE_COMMAND_PORT)) {
        printf("ERROR: UDPSocket::open(port=%d) failed. (%s, %d)\n", ARDRONE_COMMAND_PORT, __FILE__, __LINE__);
        return -1;
    }

    // 初期位置が水平面と伝える
    //sockCommand.sendf("AT*FTRIM=%d,\r", seq++);

    // 緊急ロックの解除
    //sockCommand.sendf("AT*REF=%d,290717952\r", seq++);

    // メインループ
    while (!GetAsyncKeyState(VK_ESCAPE)) {
        // Spaceで離陸
        if (KEY_PUSH(VK_SPACE))  sockCommand.sendf("AT*REF=%d,290718208\r", seq++);

        // Enterで着陸
        if (KEY_PUSH(VK_RETURN)) sockCommand.sendf("AT*REF=%d,290717696\r", seq++);

        // 速度指令
        double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
        if (KEY_DOWN(VK_UP))    vx =  0.5;
        if (KEY_DOWN(VK_DOWN))  vx = -0.5;
        if (KEY_DOWN(VK_LEFT))  vr =  0.5;
        if (KEY_DOWN(VK_RIGHT)) vr = -0.5;
        if (KEY_DOWN('Q'))      vz =  0.5;
        if (KEY_DOWN('A'))      vz = -0.5;

        // 動く
        float v[4] = {-vy, -vx, vz, -vr};
        int mode = (fabs(vx) > 0.0 || fabs(vy) > 0.0);
        sockCommand.sendf("AT*PCMD=%d,%d,%d,%d,%d,%d\r", seq++, mode, *(int*)(&v[0]), *(int*)(&v[1]), *(int*)(&v[2]), *(int*)(&v[3]));

        // ウェイト
        Sleep(30);
    }

    // ポートを閉じる
    sockCommand.close();

    // WSA終了
    WSACleanup();

    return 0;
}

udp.h
udp.cpp

AR.Droneと通信するときは以下の様な警告が出る場合があります。「許可」に変更しましょう。



ATコマンドは10ms以下の周期で送ると正しく認識されません。また、緊急停止時やバッテリーの残量が低い(20％以下)と、いくらコマンドを送っても動きません。

そのような場合はナビゲーションデータを見ればいいのですが、それはまた次回ということで。

UDP通信クラス

AR.DroneはUDP(User Datagram Protocol)で通信を行っています。

今回は下準備ということでこのUDPを取り扱うクラスを作ってみましょう。

UDPは通信の信頼性は高くありませんが、通信速度が速いのが特徴です。

処理の流れは以下の通りです。
・socket()でUDPのソケットを作る
・bind()でクライアント側のポートを開く
・ioctlsocket()で非同期モードに設定
・sendto()でサーバに送信
・recvfromで受信
・closesocket()で終了



これでAR.Droneと通信する準備ができました。

次回は実際にコマンドを送ってみようと思います。

AR.Droneを動かそう（バージョン情報の取得）

今回からAR.Droneのハックを始めていこうと思います。

AR.Droneは現在1.0と2.0があり、両方に対応するにはバージョン情報を正しく取得する必要があります。

リファレンスにはFTPのポート5551番を使っていると書いてあります。

Windows環境ならWinINetが使えるので、これで通信してみましょう。

やり方は、
・InternetOpen()で初期化
・InternetConnect()でAR.Droneのアドレス(192.168.1.1)のポート5551を開く
・FtpGetFile()でversion.txtを取得
・fopen()で取得したファイルを開く
・fscanf()でバージョン情報を読み出す

と、まぁこんな感じです。
上記の例ではInternetSetOption使って接続のタイムアウトを短く設定しています。

AR.Droneを接続していない場合はエラーが出ます。
Wifiの設定で「ardrone_xxxxxx」を見つけて接続しましょう。

他のアプリケーションでAR.DroneのIPアドレスを変更した場合は、ARDRONE_DEFAULT_ADDRをそれと同じ値に設定してください。

AR.Droneを動かそう（導入）

AR.Droneを研究で使おうと考えている人は増えているのではないでしょうか？

そして「どうやって開発するのか」は誰もが気になるところです。

公式からSDK（ソフトウェア開発キット）が公開されているので、これを使うのが近道なのですが...

バージョン2.0でWin32がサポートされなくなってしまったのが何とも痛いところです。
まぁ、あんなビルドもマトモに通らない設計の破綻したライブラリとか使う人いませんよね。



それなら自分で作ってしまおう、ということで早速やっていきましょう。

1. Visual Studioのインストール
バージョンは何でも良いのでインストール。

2. 公式SDKのダウンロード
2012年9月現在の最新版はv2.0。とりあえず公式からダウンロード。
デベロッパーガイドを取り出したら あとは使わないのでゴミ箱に入れてしまいましょう。

3. FFmpegのダウンロード
後で動画のデコードに使うので、Zeranoe FFmpeg buildsから「Latest」と書いてある(Shared)と(Dev)のライブラリをダウンロード。

4. common.hの修正
ffmpeg\include\libavutil\common.hはこのままだとコンパイルが通りません。
av_clipl_int32_c()関数をこのように書き換えるといいみたいです。

static av_always_inline av_const int32_t av_clipl_int32_c(int64_t a)
{
    if ((a+0x80000000u) & ~0xFFFFFFFFui64) return (a>>63) ^ 0x7FFFFFFF;
    else                                   return (int32_t)a;
}

5. stdint.hのダウンロード
VectorからVC++用のstdint.hをダウンロード。FFmpegのincludeディレクトリに入れておきましょう。

6. OpenCVのダウンロード
画像の表示用に使います。Source Forgeから最新版をダウンロードしてインストール。
各モジュールのinclude全部と\opencv\build\x86の中にある.libと.dllをコピー。

vc9と書いてあるのがVS2008用で、vc10と書いてあるのがVS2010用です。
VS2005を使ってる人はCMakeを使ってビルドする必要があります。

7. 新しいプロジェクトを作る
Visual Studioを起動して、「ファイル」→「新規作成」→「プロジェクト」と辿る。



「Visual C++」の「Win32コンソールアプリケーション」を選択して次へ。



「空のプロジェクト」を選択して完了。こうしないと余計なファイルが作られてしまいます。

プロジェクトを作ったら、ヘッダとライブラリへのパスを通して入力ライブラリを指定しましょう。
opencv_calib3d242.lib
opencv_contrib242.lib
opencv_core242.lib
opencv_features2d242.lib
opencv_flann242.lib
opencv_highgui242.lib
opencv_imgproc242.lib
opencv_legacy242.lib
opencv_ml242.lib
opencv_nonfree242.lib
opencv_objdetect242.lib
opencv_photo242.lib
opencv_stitching242.lib
opencv_video242.lib
opencv_videostab242.lib
avcodec.lib
avdevice.lib
avfilter.lib
avformat.lib
avutil.lib
postproc.lib
swresample.lib
swscale.lib

特に何もやってませんが今回はこれまで。

説明が適当なのは仕様です。

Visual Studioの設定

皆さんプログラムは何を使って作っていますか？

言語はC/C++で、コンパイラはGCCかMSVCが多いのではないでしょうか？

本ブログでは主にVisual Studioを用いています。べ、別にmakeが書けないわけじゃないんだからね!

長く使ってきたので、もう慣れましたのですが、やっぱりパスの設定とか面倒ですよね。

よくある参考書では、こんな感じに「ツール」→「オプション」で設定しますが...



これはあまりオススメできません。

別のPCで作業したり、再インストールやら何やらで環境が変わると全部設定し直す必要があるからです。
Visual Studio 2010からは非推奨になってますし。

必要なライブラリを1個のフォルダにまとめて、



プロジェクトの「プロパティ」で設定するのが良いと思います。

リンクするライブラリをプロパティで設定するか、#pragmaで設定するかは人によります。

ディレクトリの設定以外では、エディタの設定でタブを「空白の挿入」に切り替えて、



さらに行番号を表示するように変更すると見やすくなるでしょう。



え？何？AR.Drone？ｱｰｱｰｷｺｴﾅｲ