このブログは、大國魂（ITブログ） Advent Calendar 2018 - Adventar の8 日目です。

Tello とは

わずか80gのおもちゃドローンですがホバリングの安定性や操作性がとても素晴らしいです。十数回、壁に激突していますが元気です。そんなTelloはプログラミングで動かすことができます。

https://store.dji.com/jp/shop/tello-series

Tello をプログラミングで動かすには

Tello の公式サイトにある SDKのドキュメントに使用できるコマンドが記載されています。今回使用しているコマンドは SDK v1.3 がベースになります。

Tello SDK Documentation (v1.3)

https://dl-cdn.ryzerobotics.com/downloads/Tello/20180910/Tello%20SDK%20Documentation%20EN_1.3.pdf

Tello SDK では Tello とWifi で接続し UDP Socketで通信します。アプリケーションはテキストコマンドを送信することで Tello をコントロールできるようになります。

今回作ったアプリケーション

コードは GitHub に上げました。 github.com

実際の Tello の動き

動画では黄色い物体を追跡しています。

www.youtube.com

もし試しに使われる際は、安全には十分に注意して周りに人がいない場所で飛ばしてください。

Telloの可動範囲を制御する設定は、設定ファイル（tello.cfg）の [tacking] セクションにある position_limit です。トラッキング処理で追跡対象をロストした場合、意図せずTelloが動き続ける可能性があるので、値を大きくする際は十分に気を付けてください。

[tracking]
# !! important !! set the motion limit range of Tello for your safety 
#  (x, y, z (cm), rotate (dgree))
position_limit = (300, 300, 300, 180)

アプリケーションの構成

WebUIと Tello コントロール用の API 部分は Flaskで実装しました。

ブラウザ (Web UI ) から各ボタンをクリックすると特定の URI に POSTでリクエストします。
リクエストを受け取った Flask App は、Tello SDK を利用して UDP Socket 経由でコマンドを送信し、その結果を受信します。
コマンドの結果は、Flask からブラウザへ非同期に反映されます。

Video Streaming では Tello からの Streaming を OpenCV の VideoCapture で受けてブラウザで表示します。
Object tracking ではOpenCV を使って一定の間隔でトラッキング処理を行います。さらにその結果をTello を動作させるプログラム（tellolib) に渡し、Tello SDK と連携させます。

Object tracking と Tello との連携

Object tracking の動作は OpenCV チュートリアルのサンプルコードを使って確認することができます。このトラッキングの結果と連携して Tello を動かすための仕組みは以下のようなものです。

検知した物体は青枠で囲まれます。
赤枠は Tello が物体を追跡する境界です。赤枠を対象物体が超えるとその方向にTello が動くようにコマンドを送信します。結果的に赤枠内に収まるように動作させます。
黄色枠は、物体の奥行きを図る基準として使います。対象物体の青枠面積と基準とする黄色枠面積の平方根の割合によって Tello を前後に動かしています。
Web UI ではトラッキングモードをリアルタイムで変更できるようにしています（Stream only, Test mode , Tracking mode) 。

技術的な話（悩んだところ）

トラッキング処理のフレーム調整

Tello のストリーミングをそのままトラッキング処理をするとカクつきが激しいのでフレームをリサイズ (320 * 240)してトラッキング処理に渡しています。

camera.py

    def get_frame(self, stream_only, is_test, speed):
        .
        .
        ret, self.frame = self.video.read()
        self.frame = cv2.resize(
            cv2.flip(self.frame, flipcode), (frame_prop[0], frame_prop[1]))
        if not self.stream_only:
             if not self.t.is_alive():
                self.t = threading.Thread(
                    target=self._tracking, args=(ret, self.frame))
                self.t.start()

トラッキング処理の間隔

前回のロボットアームの時とは違い、Tello では一定間隔でトラッキング処理を実行しています。つまりフレーム毎にトラッキング処理を実行するのではなく、意図的にフレームをスキップしています。理由は、Tello は動作するタイミングで通常のストリーミングでもフレームが飛ぶ傾向があるためです。例えば短時間に大量のコマンドを送信して Tello が動作しようとすると、フレーム飛びが多発しトラッキングどころではなくなります。トラッキング処理間隔は tello.cfg の [tello] セクションで指定できます。

[tracking]
track_interval = 0.06

Tello へのコマンド送信の間隔

トラッキング処理とは別にコマンド送信の間隔も制御できるようにしています。これは、物体をTelloが追跡するときに動き過ぎないようにするための設定です。下の例では垂直、水平、前後動作は 1.0 秒間隔、回転動作は 0.1 秒間隔です。例えば Tello に "left 20" とコマンドを送ると、左方向に 20cm 動くわけですが 1.0 秒間隔を置く（スレッドが生きているので他のスレッドが起動しない）ので、連続して送信するのを防ぎます。

[tello]
move_interval = 1.0
rotate_interval = 0.1

でも、この間隔を大きくし過ぎると動作が緩慢になってしまいます。色々なパターンを試しましたがベストな設定が分かっていません。理想はこういった調整をしなくてもいい感じに動いてくれることなので、そもそものこのやり方が良くないんだと思っています。

水平方向と回転の動きの使い分け

Tello は上下左右、前後、回転の動作をしますが、対象物体が水平方向に動いたとき、左右に動かすのか、回転させるのかは迷いました。現時点では、物体の動いた距離によって動作を変えるようにしています。トラッキングの結果から「1フレーム前の位置」と「現在の位置」の差分の割合 (move_ratio) を計算し、それが閾値を下回る場合は回転、そうでない場合は水平方向の動作としました。

camera.py

    def _calc_move_ratio(self, track_window, track_window0):
        x, y, w, h = track_window
        x0, y0, w0, h0 = track_window0
        diff = (x0 - x, y0 - y, w0 - w, h0 - h)
        move_ratio = (round(diff[0] / self.video_prop[0], 5),
                      round(diff[1] / self.video_prop[1], 5))
        return move_ratio

tellolib.py

    def motion(self, track_window, track_area_ratio, move_ratio, margin_window,
               is_test):
        .
        .
        if x < xmin:
            if abs(move_ratio[0]) < move_ratio_threshold[0]:
                if self.rotate < self.rotate_limit:
                    command = "cw"
            elif self.sent_command not in ('cw', 'ccw'):
                if self.xpos > self.xpos_limit * -1:
                    command = "right"
        .
        .

ブラウザと Flask の非同期通信

ブラウザのリロードなし(非同期)で、Tello からのレスポンスを表示したりトラッキングモードを切り替えたいと思って調べていたら、 Ajax の post を使えばできそうだと分かりました。今さらながら jQueryの手軽さを体感しました。今後色々と使えそうです。

tello.js

$(function () {
    $('.btn').on('click', function () {
        var command = JSON.stringify({ "command": $('#' + $(this).attr('id')).val() });
        if (info_cmd.includes(JSON.parse(command).command)) {
            url = '/info';
    .
    .
        post(url, command);
    });
    function post(url, command) {
        $.ajax({
            type: 'POST',
            url: url,
            data: command,
            contentType: 'application/json',
            timeout: 10000
        }).done(function (data) {   
            < 成功したときの処理 >
        }).fail(function (jqXHR, textStatus, errorThrown) {
            < 失敗したときの処理 >
        });
        return false;
    }
});

Tello からのレスポンスの反映

Tello へ送信したコマンドのレスポンスは非同期で（スレッドで）受信します。コマンドによって応答のタイミングが違うので、送信したコマンドと結果が必ずしも一致しないことがあります。送信コマンドと応答を紐づける術がわからないので今のところどうしようもない部分です。

app.py (Tello からのレスポンスを受け取っている部分）

    """ Create a UDP socket to send and receive message with tello """
    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
        host = '0.0.0.0'
        port = 9000
        s.bind((host, port))

        def recv():
            global tello_response
            while True:
                try:
                    tello_response, server = s.recvfrom(1518)
                    tello_response = tello_response.decode(encoding="utf-8")
                    logger.info("res:{}".format(tello_response))
                except Exception:
                    print('\nExit . . .\n')
                    break

        recvThread = threading.Thread(target=recv)
        recvThread.start()

Tello からの Video Stream 受信

Tello SDK のドキュメントには以下のようにあります。

Receive Tello Video Stream Tello IP: 192.168.10.1 ->> PC/Mac/Mobile UDP Server: 0.0.0.0 UDP PORT:11111 Remark4: Set up a UDP server on PC, Mac or Mobile device and listen the message from IP 0.0.0.0 via UDP PORT 11111.
Remark5: Do Remark2 if you haven’t. Then send “streamon” command to Tello via UDP PORT 8889 to start the streaming.

Telloから Video Stream を受信するには "streamon" コマンドを送信した後、 UDP port 11111 でストリーミングを待ち受ける必要があります。

UDP Socketを作って bind する必要があると思ったのですが、明示的な bind の記述は必要なく以下のコードで動作します。 cv2.VideoCapture('udp://127.0.0.1:11111') が実行されるタイミングで UDP 0.0.0.0 :11111 でリッスンします。OpenCV のドキュメントやソースを見ても All Address でリッスンする部分は確認できず、今でもしっかり理解できていません。

camera.py

class VideoCamera(object):
    def __init__(self, socket, algorithm, target_color, stream_only, is_test,
                 speed):
        self.video = cv2.VideoCapture('udp://127.0.0.1:11111')

Tello コントロール用の REST な API について

API Key のようなアクセス制限があるわけではなく APIと呼ぶのはどうかと思いますが、Tello を動かす URI を提供しているという点で APIと書きました。一応 curl でも Telloのステータスを取ることができます。

URI	説明
/info	バッテリー、スピード、温度などの Tello コマンドを受付、結果を返す
/tellooo	Tello のコントローラコマンド受付
/tracking	トラッキングモード切替

$ curl -s -X POST localhost:5000/info -H "Content-Type: application/json" -d '{"command": "battery?"}'
{
  "ResultSet": "{\"command\": \"battery?\", \"result\": \"26\", \"connected\": false, \"streamon\": false}"
}

まとめ

正直なところ Telloでのトラッキングの精度はいまいちです。ロボットアームで追跡処理を実装した時に解像度: 320(w) x 240(h) フレームレート: 16 fps でまずまずの精度だったので、このプログラムでもフレームのリサイズ (320 * 240 ) やトラッキング間隔 (0.06 ≒ 16fps相当) で設定していますが、Tello が動作したタイミングでフレームが飛んでしまい追跡対象をロストすることが多いです。Telloに「追跡させる」というより、「追跡しやすいように対象物体を動かしてあげる」という感じです。

初めはCLIで作っていたのですがObject Tracking を別プロセスで起動したり都度ブラウザ開いたりが面倒なのでWeb UIに統一しました。結果としてリアルタイムでトラッキングモードを切り替えできるようになったので満足しています。

本来の機能を活かしつつ、自分なりの違った楽しみ方ができるのがプログラマブルなおもちゃの良いところだと思います。Tello に費やした膨大な時間が報われますように。

参考

FlaskでVideo Streamingする仕組み

https://blog.miguelgrinberg.com/post/video-streaming-with-flask https://github.com/miguelgrinberg/flask-video-streaming

Object Tracking (camshift)

https://github.com/opencv/opencv/tree/master/samples/python

kodamapのブログ

Tello で Object tracking

このブログは、大國魂（ITブログ） Advent Calendar 2018 - Adventar の8 日目です。

Tello とは

Tello をプログラミングで動かすには

今回作ったアプリケーション

実際の Tello の動き

アプリケーションの構成

Object tracking と Tello との連携

技術的な話（悩んだところ）

トラッキング処理のフレーム調整

トラッキング処理の間隔

Tello へのコマンド送信の間隔

水平方向と回転の動きの使い分け

ブラウザと Flask の非同期通信

Tello からのレスポンスの反映

Tello からの Video Stream 受信

Tello コントロール用の REST な API について

まとめ

参考

このブログは、大國魂（ITブログ） Advent Calendar 2018 - Adventar の8 日目です。

Tello とは

Tello を プログラミングで動かすには

今回作ったアプリケーション

実際の Tello の動き

アプリケーションの構成

Object tracking と Tello との連携

技術的な話（悩んだところ）

トラッキング処理のフレーム調整

トラッキング処理の間隔

Tello へのコマンド送信の間隔

水平方向と回転の動きの使い分け

ブラウザ と Flask の非同期通信

Tello からのレスポンスの反映

Tello からの Video Stream 受信

Tello コントロール用の REST な API について

​まとめ

参考

Tello をプログラミングで動かすには

ブラウザと Flask の非同期通信

まとめ