LoginSignup
19
23

More than 3 years have passed since last update.

@hsgucci

顔検出プログラム

Last updated at Posted at 2019-10-16

はじめに

このページは，

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

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

概要

OpenCVの入門と言えば，なんか「顔検出」が定番って雰囲気ありますよね．
とりあえず lena画像 で顔検出する，みたいな．
lena_std-face.jpg
今回はこの顔検出の機能を使って，Telloで顔を追いかける制御をしてみます．

具体的には，この動画の様に，自分を追いかけさせます．(スマホを顔の横に置いて撮影しました)


動画中に写っているノートＰＣ(Macbook ProにUbuntu16.04を入れたもの)が，画像処理をしている母艦です．
　

顔検出を行うと，顔の左上座標(x,y)と，幅w,高さhが求まるので，下図左の様に顔の中心座標が計算可能です．
cv_face_strate.png
Telloの映像は元々960x720なのですが，本企画記事ではcv2.resizeで常に480x360にしています(上図右)．この画面の中心座標(240,180)を目標値として，顔の中心座標が移動するようにTelloにコマンドを送ります．
また，顔の大きさw(wとhは正方形で常に同じ値が入る)が一定の大きさになるように，Telloの前後移動も制御します．

今回は，画像中の座標を使った直接的な「ビジュアルフィードバック」を行うので，
「カメラの歪みが〜」
「ピクセルと距離(ミリメートル)との変換が〜」
「カメラ座標系とワールド座標系の関係が〜」
とかの計算処理は行いません．
入門なのでシンプルに「画面の中心に被写体が居れば良い」だけを制御します．

画像処理を語るとスグに「座標系の変換が云々」と言って人間の単位系(メートル系)に変換して考えたがる人がいますが，別にTelloの単位系(ピクセル)のままでフィードバックしても問題ありません．
目的に応じて柔軟に考えるべきです．
今回は「ROSでTelloの自己位置推定をしたい」とかのヘビーなミッションではないですから(^_^;;

前提条件

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

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

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

Telloで顔検出するプログラム

ディレクトリの作成

まずは，Tello-Pythonディレクトリの下に，新しいディレクトリTello-CV-faceを作ります．

Tello-CV-faceディレクトリを作成
$ cd ~/Tello-Python/
$ mkdir Tello-CV-face
$ cd Tello-CV-face

顔検出の準備

PythonのOpenCVで顔検出(顔認識)をする，という参考例は下記以外にも非常に沢山あります．

参考例：

Python OpenCV 顔認識などのキーワードで検索してみてください．

顔検出用の学習済み分類器データのダウンロード

様々なサイトを読んで気がつくことは，顔のカスケード分類器であるhaarcascade_frontalface_alt.xmlの置き場所が異なることです．

顔検出器の様々なフルパス
cascPath = '/usr/local/opt/opencv/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml'
cascPath = '/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_alt.xml'
cascPath = '/usr/local/lib/python2.7/dist-packages/cv2/data/haarcascade_frontalface_alt.xml'

これは，OpenCVのインストールを何で行ったかで異なってきます．
apt-get installやpip installでインストールした場合にも，それぞれ異なります．
OpenCVのソースから自分でビルド&インストールした場合も異なります．

ややこしいので，今回は作業ディレクトリに直接原本を置く方法で行きます．

以下のリンクを右クリックし，[名前を付けて保存]などの機能でTello-CV-faceにダウンロードしてください．

　　haarcascade_frontalface_alt.xml

作業ディレクトリに分類器ファイルがあれば，

カレントディレクトリにあるファイルを使う
cascPath = 'haarcascade_frontalface_alt.xml'

と書くだけで済みます．

ファイルをコピー

tello.pyとlibh264decoder.soを，スケルトンのTello-CV-coreからコピーしてきましょう．

重要なファイルをコピー
$ cp ../Tello-CV-core/tello.py ./
$ cp ../Tello-CV-core/libh264decoder.so ./

main.pyもTello-CV-coreからコピーしておけば，書き加えるだけなので作業が楽ですね．

main.pyはスケルトンをコピー
$ cp ../Tello-CV-core/main.py ./

main.py

プログラム本体であるmain.pyは，スケルトンプログラムに書き加える形で作成しました．

書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
ここ　を右クリックして[名前を付けて保存]機能でファイル保存してください．

main.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello        # tello.pyをインポート
import time         # time.sleepを使いたいので
import cv2          # OpenCVを使うため

# メイン関数
def main():
    # カスケード分類器の初期化
    cascPath = 'haarcascade_frontalface_alt.xml'    # 分類器データはローカルに置いた物を使う
    faceCascade = cv2.CascadeClassifier(cascPath)   # カスケードクラスの作成

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

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    cnt_frame = 0   # フレーム枚数をカウントする変数
    pre_faces = []  # 顔検出結果を格納する変数
    flag = 0        # 自動制御をON/OFFするフラグ

    #Ctrl+cが押されるまでループ
    try:
        while True:
            # (A)画像取得
            frame = drone.read()    # 映像を1フレーム取得
            if frame is None or frame.size == 0:    # 中身がおかしかったら無視
                continue 

            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            small_image = cv2.resize(image, dsize=(480,360) )   # 画像サイズを半分に変更

            cv_image = small_image  # ウィンドウ表示画像の名前はcv_imageにする

            # 5フレームに１回顔認識処理をする
            if cnt_frame >= 5:
                # 顔検出のためにグレイスケール画像に変換，ヒストグラムの平坦化もかける
                gray = cv2.cvtColor(small_image, cv2.COLOR_BGR2GRAY)
                gray = cv2.equalizeHist( gray )

                # 顔検出
                faces = faceCascade.detectMultiScale(gray, 1.1, 3, 0, (10, 10))

                # 検出結果を格納
                pre_faces = faces

                cnt_frame = 0   # フレーム枚数をリセット


            # 顔の検出結果が空なら，何もしない
            if len(pre_faces) == 0:
                pass
            else:   # 顔があるなら続けて処理
                # 検出した顔に枠を書く
                for (x, y, w, h) in pre_faces:
                    cv2.rectangle(cv_image, (x, y), (x+w, y+h), (0, 255, 0), 2)

                # １個めの顔のx,y,w,h,顔中心cx,cyを得る
                x = pre_faces[0][0]
                y = pre_faces[0][1]
                w = pre_faces[0][2]
                h = pre_faces[0][3]
                cx = int( x + w/2 )
                cy = int( y + h/2 )

                # 自動制御フラグが1の時だけ，Telloを動かす
                if flag == 1:
                    a = b = c = d = 0   # rcコマンドの初期値は0

                    # 目標位置との差分にゲインを掛ける（P制御)
                    dx = 0.4 * (240 - cx)       # 画面中心との差分
                    dy = 0.4 * (180 - cy)       # 画面中心との差分
                    dw = 0.8 * (100 - w)        # 基準顔サイズ100pxとの差分

                    dx = -dx # 制御方向が逆だったので，-1を掛けて逆転させた

                    print('dx=%f  dy=%f  dw=%f'%(dx, dy, dw) )  # printして制御量を確認できるように

                    # 旋回方向の不感帯を設定
                    d = 0.0 if abs(dx) < 20.0 else dx   # ±20未満ならゼロにする
                    # 旋回方向のソフトウェアリミッタ(±100を超えないように)
                    d =  100 if d >  100.0 else d
                    d = -100 if d < -100.0 else d

                    # 前後方向の不感帯を設定
                    b = 0.0 if abs(dw) < 10.0 else dw   # ±10未満ならゼロにする
                    # 前後方向のソフトウェアリミッタ
                    b =  100 if b >  100.0 else b
                    b = -100 if b < -100.0 else b


                    # 上下方向の不感帯を設定
                    c = 0.0 if abs(dy) < 30.0 else dy   # ±30未満ならゼロにする
                    # 上下方向のソフトウェアリミッタ
                    c =  100 if c >  100.0 else c
                    c = -100 if c < -100.0 else c

                    # rcコマンドを送信
                    drone.send_command('rc %s %s %s %s'%(int(a), int(b), int(c), int(d)) )

            cnt_frame += 1  # フレームを+1枚

            # (X)ウィンドウに表示
            cv2.imshow('OpenCV Window', cv_image)   # ウィンドウに表示するイメージを変えれば色々表示できる

            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出，プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                flag = 0    # フィードバックOFF
                drone.send_command('rc 0 0 0 0')    # ラジコン指令をゼロに
                drone.land()    # 着陸
                time.sleep(3)   # 着陸するまで他のコマンドを打たないよう，ウェイトを入れる
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降
            elif key == ord('1'):
                flag = 1                    # フィードバック制御ON
            elif key == ord('2'):
                flag = 0                    # フィードバック制御OFF
                drone.send_command('rc 0 0 0 0')    # ラジコン指令をゼロに

            # (Z)5秒おきに'command'を送って、死活チェックを通す
            current_time = time.time()  # 現在時刻を取得
            if current_time - pre_time > 5.0 :  # 前回時刻から5秒以上経過しているか？
                drone.send_command('command')   # 'command'送信
                pre_time = current_time         # 前回時刻を更新

    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

    drone.send_command('streamoff')
    # telloクラスを削除
    del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":      # importされると"__main__"は入らないので，実行かimportかを判断できる．
    main()    # メイン関数を実行

プログラムの実行

プログラム本体はmain.pyです．

プログラムの実行
$ python main.py

今までと同様にctrl+cを押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう．

操作系

操作系は以下の様になっています．
1キーでビジュアルフィードバック制御がON(有効)になり，
2キーでOFF(無効)になります．
cv_face_key.png

自動制御がONになると，Telloは左右旋回・上下移動・前後移動の３つを行います．

操作手順

  1. tキーで離陸させる．
  2. 上下前後左右の移動キーで，顔が連続して認識できる位置(安全な位置)までTelloを手動操作する．
  3. 1キーを押してフィードバック制御を開始させる．
  4. 上下・左右・前後に顔を動かして，Telloが追従してくる事を確認する．
  5. もしTelloが意図しない方向へ流れ始め(暴走し)たら，2キーを押して制御を終了させ，移動キーでTelloを止める．
  6. lキーで着陸させる．

Tello SDKのrcコマンドを使って操作しているので，機体が流れ始めた際に止めるのは手動操作だけです．（移動コマンドは応答が遅いので使っていません）

実行結果

自動制御がOFFの場合は，これまでと同じです．

自動制御がONの間は，以下の様な画面表示が出ます．

実行結果
dx=38.400000  dy=-7.600000  dw=10.400000
>> send cmd: rc 0 10 0 38
dx=38.400000  dy=-7.600000  dw=10.400000
>> send cmd: rc 0 10 0 38
dx=38.400000  dy=-7.600000  dw=10.400000
>> send cmd: rc 0 10 0 38自動制御がONになると，Telloは左右旋回・上下移動・前後移動の３つを行います．
dx=38.400000  dy=-7.600000  dw=10.400000
>> send cmd: rc 0 10 0 38
dx=38.400000  dy=-7.600000  dw=10.400000
>> send cmd: rc 0 10 0 38
dx=60.000000  dy=-4.000000  dw=6.400000
>> send cmd: rc 0 0 0 60
dx=60.000000  dy=-4.000000  dw=6.400000
>> send cmd: rc 0 0 0 60
dx=60.000000  dy=-4.000000  dw=6.400000
>> send cmd: rc 0 0 0 60
dx=60.000000  dy=-4.000000  dw=6.400000
>> send cmd: rc 0 0 0 60
dx=60.000000  dy=-4.000000  dw=6.400000
>> send cmd: rc 0 0 0 60

rcコマンドを使って，Telloを動かしていることがわかります．

main.pyの解説

メイン関数以外は，スケルトンプログラムと同じなので，説明は割愛します．

メイン関数

メイン関数の中身は大きく分けて３つの部分に分かれています．
「初期化」「ループ」「終了処理」です．

メイン関数
# メイン関数本体
def main():
    初期化部

    ループ部

    終了処理部

それぞれ解説していきます．

初期化部

初期化処理部
    # カスケード分類器の初期化
    cascPath = 'haarcascade_frontalface_alt.xml'    # 分類器データはローカルに置いた物を使う
    faceCascade = cv2.CascadeClassifier(cascPath)   # カスケードクラスの作成

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

    current_time = time.time()  # 現在時刻の保存変数
    pre_time = current_time     # 5秒ごとの'command'送信のための時刻変数

    time.sleep(0.5)     # 通信が安定するまでちょっと待つ

    cnt_frame = 0   # フレーム枚数をカウントする変数
    pre_faces = []  # 顔検出結果を格納する変数
    flag = 0        # 自動制御をON/OFFするフラグ

まず最初に，顔検出に必要なカスケード分類器データの読込を行っています．
カレントディレクトリにファイルをダウンロードしてあるので，OpenCVのディレクトリへのフルパスを書く必要はありません．

Telloクラスの呼び出し時の引数はcommand_timeout=.01でタイムアウトまで10ミリ秒なのは，いつもと同じです．

5秒おきにcommandを送信する機能のための変数も使います．

顔検出の画像処理は５フレームに１回の割合で行っているので，そのカウント変数，および検出結果の保存用変数を作ってあります．

ポイントはflag = 0で定義してあるフラグです．Pythonプログラム実行直後からTelloの制御を開始してしまうと，離陸前に映り込んだ背景や床などを顔として誤検出した際に暴走してしまいます．したがって，人の顔がカメラに映るまでの離陸・移動は人間が操作し，キーボード入力でTelloの自動制御を開始させます．それを管理するのがこのflagです．flag==1になると手動操縦を自動操縦に切り替え，flag==0で手動操縦に戻ります．

ループ部

while Trueで永久ループを作り，ctrl+cを検知をtry exceptでやるのは今までと同様です．

ループ部を更に(A),(B),(X),(Y),(Z)の5ブロックに分け，変更のあるブロックだけ説明します．

ループ部
    #Ctrl+cが押されるまでループ
    try:
        while True:
            (A) 画像取り込み - 変更なし
            (B) 画像処理部
            (X) 画像ウィンドウ表示 - 変更なし
            (Y) キー入力部
            (Z) 5秒ごとの'command'送信 - 変更なし
    except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
        print( "SIGINTを検知" )

(A),(X),(Z)は特に変更がないので，最も重要な(B)から説明します．

(B)画像処理ブロック

(B)画像処理ブロック①
            # (B)ここから画像処理
            image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)      # OpenCV用のカラー並びに変換する
            small_image = cv2.resize(image, dsize=(480,360) )   # 画像サイズを半分に変更

            cv_image = small_image  # ウィンドウ表示画像の名前はcv_imageにする

まずは，取り込んだ画像をOpenCVに適したBGRのカラー並びへと変更し，画像サイズを480x360に縮小しています．
顔認識結果を描画して(X)で表示するために，縮小画像をcv_imageとしてコピーしています．

(B)画像処理ブロック②
            # 5フレームに１回顔認識処理をする
            if cnt_frame >= 5:
                # 顔検出のためにグレイスケール画像に変換，ヒストグラムの平坦化もかける
                gray = cv2.cvtColor(small_image, cv2.COLOR_BGR2GRAY)
                gray = cv2.equalizeHist( gray )

                # 顔検出
                faces = faceCascade.detectMultiScale(gray, 1.1, 3, 0, (10, 10))

                # 検出結果を格納
                pre_faces = faces

                cnt_frame = 0   # フレーム枚数をリセット

5フレームに1回だけ顔検出処理を行うように，カウントしたフレーム枚数を見ています．
ＰＣの性能によっては「10フレームに１回」とか「3フレームに1回」とかに調整してください．
このif文の数値を大きくすればするだけCPU負荷を下げることはできますが，その代わりに顔の現在位置のセンシングに遅延が起こるわけですから，Telloの移動がカクカクしてぎこちなくなります．

顔検出の手続きは以下の手順です．

  1. cv2.cvtColor：　BGR画像をグレイスケール画像へ変換
  2. cv2.equalizeHist：　グレイスケール画像にヒストグラム平均化処理をかける
  3. detectMultiScale：　ヒストグラム平均化画像に顔検出処理をかける

3のdetectMultiScaleの引数については，scaleFactor=1.1, minNeighbors=3, flags=0, minSize=(10,10)(=10x10画素)としています．よく使われるデフォルト値です．

　　参考：物体検出（detectMultiScale）をパラメータを変えて試してみる（scaleFactor編）

検出結果はpre_facesに格納しています．５回の内４回は検出処理を行わないので，その４回は以前の検出結果に基づいて制御を行うためです．

(B)画像処理ブロック③
            # 顔の検出結果が空なら，何もしない
            if len(pre_faces) == 0:
                pass
            else:   # 顔があるなら続けて処理
                # 検出した顔に枠を書く
                for (x, y, w, h) in pre_faces:
                    cv2.rectangle(cv_image, (x, y), (x+w, y+h), (0, 255, 0), 2)

                # １個めの顔のx,y,w,h,顔中心cx,cyを得る
                x = pre_faces[0][0]
                y = pre_faces[0][1]
                w = pre_faces[0][2]
                h = pre_faces[0][3]
                cx = int( x + w/2 )
                cy = int( y + h/2 )

この部分は１フレームに１回処理を行っています．
顔検出の結果が空なら何もせず，中身があれば描画や計算を行います．

まずは，顔検出でよくやられている，「検出した顔を矩形で囲う」という描画処理です．

cv2.rectangle：　長方形の描画

顔検出は複数の顔を検出できるのですが，今回のプログラムは最初に認識した１個目の顔をトラッキングします．
最も大きな顔を追いかけるように改良するには，検出した顔の幅wが最大の要素を取り出せば良いと思います．

１個目(インデックス番号は0)の要素から，左上座標(x,y)と顔の幅w，高さhを取り出し，顔の中心座標(cx,cy)を計算しています．この座標が(240,180)になるようにフィードバック制御をかけるのです．

(B)画像処理ブロック④
                # 自動制御フラグが1の時だけ，Telloを動かす
                if flag == 1:
                    a = b = c = d = 0   # rcコマンドの初期値は0

                    # 目標位置との差分にゲインを掛ける（P制御)
                    dx = 0.4 * (240 - cx)       # 画面中心との差分
                    dy = 0.4 * (180 - cy)       # 画面中心との差分
                    dw = 0.8 * (100 - w)        # 基準顔サイズ100pxとの差分

                    dx = -dx # 制御方向が逆だったので，-1を掛けて逆転させた

                    print('dx=%f  dy=%f  dw=%f'%(dx, dy, dw) )  # printして制御量を確認できるように

まずはflagをチェックして，Telloの制御のOn/Offを確認しています．Offの時は何もしません．なお，このflagはキーボード入力の(Y)で変更できるようにしています．

a,b,c,dはrcコマンドで送信するスティック入力量を格納する変数です．デフォルトでは動かないのでゼロにしておきます．

画面中心(240,180)と顔中心(cx,cy)，基準顔サイズ100pxと現在顔サイズwについて，

  • x軸方向の偏差(240-cx)にPゲイン0.4を掛けたものをdx
  • y軸方向の偏差(180-cy)にPゲイン0.4を掛けたものをdy
  • 顔サイズの偏差(100-w)にPゲイン0.8を掛けたものをdw

としています．
ゲインは0.1を起点として，0.1ずつ増減して調整していきました．

今回のビジュアルフィードバック制御で使ったのは，もっとも基本的なP制御です．(PID制御のP)　今回の記事の内，最も重要な部分はこの式ですね．この制御式をどのように書くかで，制御の味付けが変わってきます．PD制御やPI制御へ改良する楽しみもあるかと思います．

(B)画像処理ブロック⑤
                    # 旋回方向の不感帯を設定
                    d = 0.0 if abs(dx) < 20.0 else dx   # ±20未満ならゼロにする
                    # 旋回方向のソフトウェアリミッタ(±100を超えないように)
                    d =  100 if d >  100.0 else d
                    d = -100 if d < -100.0 else d

                    # 前後方向の不感帯を設定
                    b = 0.0 if abs(dw) < 10.0 else dw   # ±10未満ならゼロにする
                    # 前後方向のソフトウェアリミッタ
                    b =  100 if b >  100.0 else b
                    b = -100 if b < -100.0 else b


                    # 上下方向の不感帯を設定
                    c = 0.0 if abs(dy) < 30.0 else dy   # ±30未満ならゼロにする
                    # 上下方向のソフトウェアリミッタ
                    c =  100 if c >  100.0 else c
                    c = -100 if c < -100.0 else c

                    # rcコマンドを送信
                    drone.send_command('rc %s %s %s %s'%(int(a), int(b), int(c), int(d)) )

            cnt_frame += 1  # フレームを+1枚

制御式で得られたdx,dy,dwに，不感帯とソフトウェアリミッタの処理をかけています．

不感帯
デッドバンドとも言います．目標値と現在値が近い時に，何も制御しないでゼロとする処理です．今回はP制御なので，ブレーキ役のD制御がいません．そのため目標値付近で機体が振動的しないよう不感帯を大きめにとりました．
ソフトウェアリミッタ
rcコマンドは±100までの数値しか受け付けません．従って，制御式が±100を超えていた場合には-100と100に制限する必要があります．

これらの式はif文で書くと無駄に行数を食うので，Pythonの三項演算子という書式で短く書きました．

　　三項演算子(Python)

Ｃ言語の条件演算子(はてな-コロン文)みたいな，ちょいテクですね．

最後に，処理後のa,b,c,dを使ってrcコマンドで送信しています．

(Y)キー入力ブロック

(Y)のキー入力のブロックは，基本的にはTello-CV-coreと同じです．

異なる点は，

  • 着陸コマンドの処理に安定化処理を入れた
  • 1キーと2キーを押した際の処理を追加した

の２つです．

(Y)キー入力ブロック
            # (Y)OpenCVウィンドウでキー入力を1ms待つ
            key = cv2.waitKey(1)
            if key == 27:                   # k が27(ESC)だったらwhileループを脱出，プログラム終了
                break
            elif key == ord('t'):
                drone.takeoff()             # 離陸
            elif key == ord('l'):
                flag = 0    # フィードバックOFF
                drone.send_command('rc 0 0 0 0')    # ラジコン指令をゼロに
                drone.land()    # 着陸
                time.sleep(3)   # 着陸するまで他のコマンドを打たないよう，ウェイトを入れる
            elif key == ord('w'):
                drone.move_forward(0.3)     # 前進
            elif key == ord('s'):
                drone.move_backward(0.3)    # 後進
            elif key == ord('a'):
                drone.move_left(0.3)        # 左移動
            elif key == ord('d'):
                drone.move_right(0.3)       # 右移動
            elif key == ord('q'):
                drone.rotate_ccw(20)        # 左旋回
            elif key == ord('e'):
                drone.rotate_cw(20)         # 右旋回
            elif key == ord('r'):
                drone.move_up(0.3)          # 上昇
            elif key == ord('f'):
                drone.move_down(0.3)        # 下降
            elif key == ord('1'):
                flag = 1                    # フィードバック制御ON
            elif key == ord('2'):
                flag = 0                    # フィードバック制御OFF
                drone.send_command('rc 0 0 0 0')    # ラジコン指令をゼロに

着陸のlキーを押した際は，着陸コマンドを出す以外の作業もしています．
というのは，自動制御がONになっていると，landコマンドを送信した次のループでrcコマンドが送信されるため，landがキャンセルされて無かったことになってしまうからです．
したがって，「自動制御をOFF」「rcコマンドもニュートラル」にしたあとでlandコマンドを送信し，安定着陸するまで３秒間待つように強制しました．

1キーを入力したら自動制御をON，2キーを入力したら自動制御をOFFとしています．

顔検出が外れて暴走しそうになったら，2キーを押して手動制御に戻して操縦するか，lキーを押してとりあえず下ろすか判断してください．

終了処理部

終了処理はクラスを削除するまえに，streamoffのコマンドを送るようにしました．
映像のストリーミング出力をOFFにすることで，TelloのCPU負荷を減らし，少しでもバッテリーの持ちをよくさせようと言う狙いです．(あんまり変わらないけど)

終了処理部
    drone.send_command('streamoff')
    # telloクラスを削除
    del drone

おわりに

今回は,カスケード検出器を使った顔検出によって，Telloで人間の顔を追従させてみました．
部屋の中の環境によっては，顔の誤検出が頻発して危険ですので注意してください．
人の顔が写ったポスターに突撃していくかもしれません(^_^

　

19
23
2

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
19
23