「Tello_Video」の改造

#はじめに
このページは，

公式SDK「Tello-Python」を試そう

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要

DJI公式のTello用Pythonサンプルプログラム「Tello-Python」のうち，

　Tello_Video

を改造して使う方法を説明します．

Tello-Pythonには４つのサンプルがありますが，それぞれの特徴を言えば以下のようになります．

• tello_state.py　UDP受信でステータスを見るだけ
• Single_Tello_Test　UDP送信でコマンドを送って動かすだけ
• Tello_Video　映像を受け取って表示し，コマンドを送って動かすこともできる
• Tello_Video_With_Pose_Recognition　機械学習がついたTello_Video

tello_state.pyとSingle_Tello_Testでは，Telloのカメラ映像を使うことができません．
一方，Tello_Video_With_Pose_Recognitionは，機械学習が重いので応用性に乏しいです．
Telloのカメラ映像が取れて，操縦もできるTello_Videoが一番バランスが良いと思います．

カメラ映像を画像処理して，結果をフィードバックして機体を動かす，という「自律移動ロボット」の様な動作をさせるためには，Tello_Videoを改造すると良いでしょう．

今回は，

Telloから受信したカメラ画像にOpenCVで簡単な画像処理をかける

という作業をやってみたいと思います．

#前提条件

ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます．

Linuxマシンであれば　/home/(ユーザー名)/　に，Tello-Pythonというフォルダがあることになります．

詳しくは　Tello-Pythonのダウンロード　を御覧ください．

#Tello_Videoのコピー

ちゃんと動く「Tello_Video」が必要です．
（つまりlibh264decoder.soがビルド済みであること）

オリジナルのTello_Videoを書き換えてしまうと，プログラムが動かなくなってしまった時に大変なので，ディレクトリごとコピーして，コピー先を改変して行きましょう．

Tello_Videoをコピー

$ cp -R Tello_Video/ Tello_Video_PJ01/

-rまたは-Rは，ディレクトリの中身も再帰的にコピーしてくれるオプションですね．

今後は，このTello_Video_PJ01（プロジェクト01）を使います．

一応，PJ01の中身がちゃんとあるか確認しておきます．

Tello_Video_PJ01の中を確認

$ cd Tello_Video_PJ01
$ ls
LICENSE.md   img                main.py    tello_control_ui.py
README.md    install            tello.py   tello_control_ui.pyc
h264decoder  libh264decoder.so  tello.pyc

今回は，tello_control_ui.pyを書き換える話になります．

システム構成図から考えると，

「え？ main.pyが実行している本体だから，main.pyをイジるんじゃないの？」
と思ってしまいますが，そのあたりも含めて解説します．

#main.pyの解説

まずはmain.pyの中を見てみます．

main.py(日本語コメント付き)

import tello    # tello.pyをインポート
from tello_control_ui import TelloUI    # tello_control_ui.pyをインポート

# メイン関数本体
def main():

    # Telloクラスを使って，droneというインスタンス(実体)を作る
    drone = tello.Tello('', 8889) 

    # TelloUIクラスを使って，vplayerというインスタンスを作る
    # 　上記のdroneと，スナップショット保存先フォルダを引数で渡す.
    # 　なので，droneに対する操作はvplayer(TelloUIクラス)の中で行っている
    vplayer = TelloUI(drone,"./img/")    
    
    # vplayerはTkinterを使ったGUIウィンドウプログラムなので，メインループを回す必要がある
    vplayer.root.mainloop() 

# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":
    main()    # メイン関数を実行

コメント行を除けば，実質８行のプログラムで，とても短いです．

イメージとしては，

「drone(Telloクラス)を作り，vplayer(TelloUIクラス)に作業を丸投げしている」
「main.pyはループを回すだけで，それ以外の仕事はしていない」

という感じでしょうか．

###ループについて，ちょっと解説

ある単作業を行って終了して良いプログラムではなく，永続的に動き続ける必要があるプログラムでは，必ず何らかのループが回っています．

例えばArduinoのプログラミングだとsetup()とloop()という関数がありますが，loop()は永続的に呼び出される関数ですね．
これは，main関数が１回だけ走って終了してしまっては困るからです．
電源が入っている限り，ずっと走り続ける必要があるからです．
もしループがなかったら，プログラムはすぐ終了し，CPUはやることがなくなって停止します．

main関数の構造(イメージ)

void main() {
    setup();        // setupは1回だけ呼び出される

    while(1) {      // 永久ループ
        loop();     // loopは何度も呼び出される
    }
}

Processingでも，setup()とdraw()があり，draw()は何度も呼び出されます．
これも，プログラムが終了せず，ずっとグラフィックを描画し続ける必要があるからです．

Windowsでウィンドウを作るプログラミングでも，while文で書かれた「メッセージループ」が回っています．
これも，プログラムが終了せず，ボタンクリックやウィンドウ移動などのイベントメッセージが届くのを待ち続ける必要があるからです．
ループがなければプログラムはすぐ終了し，OSに制御を返してしまいます．

Win32APIのmain関数のイメージ(省略あり)

int WINAPI WinMain(略)
{
    略
    hWnd = CreateWindow(略);
    ShowWindow(hWnd, nCmdShow);
    UpdateWindow(hWnd);
    while (GetMessage(&msg, NULL, 0, 0)) {
        TranslateMessage(&msg);
        DispatchMessage(&msg);
    }
}

MQTTのPythonライブラリであるpaho-mqttでも，mqtt_client.loop_forever()の様に呼び出して，永遠にループを回し続ける使い方があります．

同じ考え方で，Linuxでもウィンドウ(GUI)を作るプログラミングでは，ボタンクリックやウィンドウ移動などのイベントを待ち続ける必要があるので，どこかでループを回す必要があります．

main.pyの場合は，それがvplayer.root.mainloop()です．
Tkinterというライブラリの中で暗黙的に定義されているループですが，これを呼ぶことでプログラムは終了せずに走り続けるのです．

#tello_control_ui.pyの解説と改造

tello_control_ui.pyは360行あるので，全てを説明すると大変です．
大事なポイントとなるvideoLoop関数に絞ります．

videoLoopとその次の_updateGUIImageを，日本語訳して適宜改変したものを以下に示します．

videoLoop関数(tello_control_ui.py)

    def videoLoop(self):
        """
        ビデオを取得するスレッド
             (原文は「Tkinterのメインループスレッド」だったけど，なんか違う．
        """
        try:   # ランタイムエラー(主にH.264のデコード失敗)を引っ掛けるためのtry except文

            # 画像取得スレッドの開始
            time.sleep(0.5)    # 0.5秒のウェイト（おそらくTelloとの通信の安定を待つ)
            self.sending_command_thread.start()    # 5秒おきに'command'を送信するスレッドを開始
                                                   # Telloは15秒間コマンドが来ないと自動着陸してしまうので．

            while not self.stopEvent.is_set():    # スレッド終了命令を検知するまで永久ループ                
                system = platform.system()    # OSが何か調べる(Win?Mac?Linux?)

                # GUIで表示するフレームの取得
                self.frame = self.tello.read()    # Telloクラスで受信＆デコードした画像をここで読み取る
                if self.frame is None or self.frame.size == 0:    # 画像データの中身が空(要はデータの取得失敗)だったら
                    continue    # 無視してwhileループ先頭へ戻る
            
                # フレームをTkinter用にPILイメージに変換する        
                image = Image.fromarray(self.frame)

                # Mac OSではTkinterのImageTK.PhotoImage関数の処理に非常に長い時間がかかる問題があったため，
                # Mac OSの場合は_updateGUIImage関数を別スレッドとして実行する様にしました
                if system =="Windows" or system =="Linux":    # WindowsとLinuxでは普通に関数を呼ぶ                
                    self._updateGUIImage(image)    # imageをGUIウィンドウのパネルに描画する

                else:    # Macではスレッドとして呼ぶ
                    thread_tmp = threading.Thread(target=self._updateGUIImage,args=(image,))    # スレッド作成
                    thread_tmp.start()    # スレッド開始
                    time.sleep(0.03)    # 1フレーム分(30FPS)の待ち時間

        except RuntimeError, e:    # ランタイムエラーを検知したら
            print("[INFO] caught a RuntimeError")    # エラーメッセージを表示

    # ウィンドウの映像を更新(videoLoopから呼ばれる)
    def _updateGUIImage(self,image):
        """
        imageオブジェクトの初期化と，GUIのパネルの更新する
        """  
        image = ImageTk.PhotoImage(image)
        # パネルがなければ，初期化をする必要がある
        if self.panel is None:
            self.panel = tki.Label(image=image)
            self.panel.image = image
            self.panel.pack(side="left", padx=10, pady=10)
        # そうでなければ，単純にパネルを更新する
        else:
            self.panel.configure(image=image)
            self.panel.image = image

##videoLoopの解説

このvideoLoop関数は，TelloUIのコンストラクタ(__init__)の中で，以下の様に呼びだされています．

TelloUIクラスのコンストラクタの一部

        self.thread = threading.Thread(target=self.videoLoop, args=())
        self.thread.start()

新しいスレッドとしてvideoLoopを登録しているので，Tkinterのメインループと同時並行で走ることになります．

そのため，videoLoopを(分かりやすくtry文を排除して)見てみると

videoLoopの冒頭

def videoLoop(self):

    # １回だけ実行される初期化処理 ≒ setup()
    time.sleep(0.5)    # 0.5秒のウェイト（おそらくTelloとの通信の安定を待つ)
    self.sending_command_thread.start()    # 5秒おきに'command'を送信するスレッドを開始

    # 永久に繰り返されるループ処理 ≒ loop()
    while not self.stopEvent.is_set():    # スレッド終了命令を検知するまで永久ループ                
        （略）

初期化処理とループ処理に別れていることが，分かります．

##画像を取り込む部分
videoLoop内のwhileループを見ていくと，

Telloクラスから画像を取り込む部分

        # GUIで表示するフレームの取得
        self.frame = self.tello.read()    # Telloクラスで受信＆デコードした画像をここで読み取る
        if self.frame is None or self.frame.size == 0:    # 画像データの中身が空(要はデータの取得失敗)だったら
            continue    # 無視してwhileループ先頭へ戻る

self.tello.read()関数で，画像を読み込んでいることが分かります．
OpenCVで例えれば，

OpenCVでビデオの取得する場合

cap = cv2.VideoCapture(0) #ビデオキャプチャの開始

while True:
    ret, frame = cap.read() #ビデオキャプチャから画像を取得

cap.read()に該当することになります．

ただし，USBカメラやIEEEカメラから画像を取るOpenCVと異なり，WiFiのストリーミングで画像を取るTelloは，画像が欠けたりすることも多いので，
self.frame is None or self.frame.size == 0の様に中身がなかった場合の処理も書き足されています．

##ウィンドウへの表示

取り込んだ画像をウィンドウへ表示する部分を，WindowsやMacという機種依存部分を無視して書くと以下の様になります．

画像フォーマットの変換と，Tkinterのウィンドウへの表示(パネルへ描画)

        # フレームをTkintery用にPILイメージに変換する        
        image = Image.fromarray(self.frame)

        self._updateGUIImage(image)    # imageをGUIウィンドウのパネルに描画する

Tkinterで作ったウィンドウに表示したいので，Tkinterに対応したPILという画像ライブラリのフォーマットに変換しています．

##videoLoopのフロー

前節の内容を図示して見ると，以下の様になります．

図中にも書いてある通り，
もしもOpenCVなどを使って画像処理をするのであれば，

• 画像読み込みの後　(そもそも画像がなければ画像処理できない)
• 画像のウィンドウ表示の前　(画像処理結果をウィンドウに表示したい)

に書くべきでしょう．
というわけで，次項では簡単な改造方法を示します．

#OpenCVの画像処理を追加

それではこれから，tello_control_ui.pyを書き換えて，比較的簡単なOpenCV関数を使って画像処理をかけてみましょう．

キーポイントは，

• self.frameにTello画像が入っている
• 最終的にimageにPIL画像を格納しておけばウィンドウに表示される

です．
従って，次のプログラムのような流れを意識しておくと良いでしょう．

画像処理結果はcv_imageに格納して，それを後でPIL画像にする

        self.frame = self.tello.read()    # 画像を読み取る

        # ここに画像処理を書く
        cv_image = （self.frameを入力画像とし，処理結果をcv_imageに代入）

        image = Image.fromarray(cv_image)    # self.frameの代わりに，cv_imageをPIL画像へ変換
        self._updateGUIImage(image)    # imageをGUIウィンドウに描画する

入力画像 self.frame を画像処理した結果を， cv_image という新しい変数に入れる様に書きましょう．
そして，今まで image = Image.fromarray(self.frame) だった文は，**image = Image.fromarray(cv_image)**になりました．

この入出力の辻褄さえ分かっていれば，後は色々なOpenCV関数を試すだけです．

##画像のサイズ変更(リサイズ)

TelloからHD(720p)で送られてくる画像は，4:3のアスペクト比なので 960 x 720ピクセルです．
これを表示させると，結構大きくて重たいことが多いです．

resize関数で，画像サイズを半分にしてみましょう．

　　OpenCV - 画像をリサイズする方法 (cv2.resize)

画像サイズを変更する

        self.frame = self.tello.read()    # Telloクラスで受信＆デコードした画像をここで読み取る
        if self.frame is None or self.frame.size == 0:    # 画像データの中身が空(要はデータの取得失敗)だったら
            continue    # 無視してwhileループ先頭へ戻る

        # ここに画像処理を書き足す
        cv_image = cv2.resize( self.frame, dsize=(480, 360) )  # self.frameの画像を480x360にリサイズして，cv_imageに格納

        image = Image.fromarray(cv_image)    # self.frameの代わりに，cv_imageをPIL画像へ変換

(入力)self.frame → cv_image → image(出力)

と変換結果が引き継がれていくことがわかると思います．

結果
ウィンドウのサイズが小さくなりました．

##画像のグレイスケール化

RGBの３色を使わず，白黒のグレイスケール画像で行う処理もあるので，書いておきます．

　　　OpenCVにてRGB画像をグレースケールに変換する

グレイスケールにする

        self.frame = self.tello.read()    # Telloクラスで受信＆デコードした画像をここで読み取る
        if self.frame is None or self.frame.size == 0:    # 画像データの中身が空(要はデータの取得失敗)だったら
            continue    # 無視してwhileループ先頭へ戻る

        # ここに画像処理を書き足す
        image1 = cv2.resize( self.frame, dsize=(480, 360) )  # self.frameの画像を480x360にリサイズして，image1に格納
        cv_image = cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY)  # image1をグレイスケール画像にして，cv_imageに格納

        image = Image.fromarray(cv_image)    # self.frameの代わりに，cv_imageをPIL画像へ変換

2つの処理をかけるので，image1という変数が増えて

(入力)self.frame → image1 → cv_image → image(出力)

と引き継がれていることがわかります．

画面が小さくなった上に，白黒になりました．

##画像の上下反転

Telloでは使うことはまずありませんが，ラズパイカメラだと設置の仕方によって使うことがあるので，書いてみました．

　　　画像を反転する

画像サイズを変更する

        self.frame = self.tello.read()    # Telloクラスで受信＆デコードした画像をここで読み取る
        if self.frame is None or self.frame.size == 0:    # 画像データの中身が空(要はデータの取得失敗)だったら
            continue    # 無視してwhileループ先頭へ戻る

        # ここに画像処理を書き足す
        image1 = cv2.resize( self.frame, dsize=(480, 360) )  # self.frameの画像を480x360にリサイズして，image1に格納
        image2 = cv2.cvtColor(image1, cv2.COLOR_BGR2GRAY)  # image1をグレイスケール画像にして，image2に格納
        cv_image = cv2.flip(image2,0)   # image2を上下反転してcv_imageに格納

        image = Image.fromarray(cv_image)    # self.frameの代わりに，cv_imageをPIL画像へ変換

３つの処理をかけるので，更にimage2という変数が増え，

(入力)self.frame → image1 → image2 → cv_image → image(出力)

と引き継がれていくことがわかります．

更に上下逆さまになりました．

#おわりに

「Tello_Video」のGUIを司る「tello_control_ui.py」その中でもビデオ関連を担っている「videoLoop」を書き換えて，OpenCVの関数が使える事を確認しました．

videoLoopを書き換えるだけでも，結構色々な処理ができます．

しかし，Tkinterで作ったGUIでわざわざ映像を表示する，というのも無駄が多いですね．

OpenCVをやっている人であれば，
「cv2.imshowで処理結果をウィンドウに出すから，Tkinterのウィンドウなんかいらないよ．」
と言うかもしれません．

次の記事では，tello.pyだけ利用してOpenCVで映像表示させようと思います．