記事の概要

TelloというDJI社のドローンを使って、人の顔を認識しながら自動飛行するプログラムを初心者が作ってみた記事になります。

開発環境

Windows10
Opencv 4.0.0
Python 3.7
anaconda3(64bit)
Tello
- https://www.ryzerobotics.com/jp/tello

仕組み

①ソケット通信

TelloはPCとWifiで接続し、ソケット通信（UDP/IP）によりデータの送受信を行います。
ソケット通信についてはこの方のブログがものすごくわかりやすいので、何度も見てました。
（ありがとうございました...orz）
https://qiita.com/init/items/5c89fa5b37b8c5ed32a4
Telloの場合、命令を送るだけなら、IPアドレスをセットして、通信用のソケットを作成し、sendtoでコマンドを送るだけで動きます。（ちなみにTelloとの接続方法はTelloの電源を入れた後、PCをネットに繋げるのと同様にTelloのWifiを探して接続させるだけです）
以下の公式SDKガイドも参考にしてください。
https://dl-cdn.ryzerobotics.com/downloads/Tello/Tello%20SDK%202.0%20User%20Guide.pdf

②TelloのSDKモード起動、Telloへの命令送信

①で作成したソケットを用いて「command」というメッセージを送り、SDKモードを起動させます。その後、ビデオストリームや離着陸などの命令をTelloに渡すことができます。

②Telloからのビデオストリーム受信

②で命令したビデオストリームデータを受信するためには、ポート11111でデータ受信の設定をしてから、OpencvのVideoCapture関数で受け取ります。VideoCapture関数の引数にはファイルパスの他、URLもとることができるため、以下のような書き方でストリームデータを持つことができます。（以下は作成したコードの一部抜粋）

VS_UDP_IP = '0.0.0.0'
VS_UDP_PORT = 11111

cap = None
udp_video_address = 'udp://@' + VS_UDP_IP + ':' + str(VS_UDP_PORT)
if cap is None:
    cap = cv2.VideoCapture(udp_video_address)

③顔認識の処理/認識結果の表示

顔認識にはOpencvの「haarcascade_frontalface_default.xml」を用いました。以下のサイトが勉強になります。
https://algorithm.joho.info/programming/python/opencv-haar-cascade-face-detection-py/

ビデオストリームのフレームデータを顔認識の分類器にかけて、顔があった場合に四角い箱を顔にかぶせるように処理しています。そして、そのフレームをimshow関数を用いて、ウィンドウ表示させています。読み込みが遅くなり、ガビガビになることを防ぐため、以下の「おまじない」もしています。

for i in range(5):
    img = cap.read()

④スレッド処理

ビデオストリームと飛行命令の処理を同時にさせるためにはスレッド処理が必要でした。また、Windowsでは動画に関する処理をサブスレッドでも実行可能だったのに対して、MACでは動画関連をメインスレッドでしなくてはならない仕様（？）があったため、（無理やり）動画部分をメインスレッドにして、飛行命令やデータ受信をサブスレッドにしました。（MACのことはよくわかりませんが、本番環境がMACに急遽変更されたので、変えなくてはならず・・・）

以下は似たようなことに対処されていたサイトです。
https://lightbulbcat.tumblr.com/post/71540856622/python%E3%81%A7opencv%E3%81%AEwaitkey%E3%81%AF%E3%83%A1%E3%82%A4%E3%83%B3%E3%82%B9%E3%83%AC%E3%83%83%E3%83%89%E3%81%98%E3%82%83%E3%81%AA%E3%81%84%E3%81%A8%E5%8B%95%E3%81%8B%E3%81%AA%E3%81%84/amp

つくってみたコード

import socket
import threading
import cv2
import time

# Telloへのアクセス用
tello_ip = '192.168.10.1'
tello_port = 8889
tello_address = (tello_ip, tello_port)

# Telloからのデータ受信用
VS_UDP_IP = '0.0.0.0'
VS_UDP_PORT = 11111

event = threading.Event()

# 顔認識用ファイルの在り処（自分の環境に合わせて入れる）
cascade_path = "・・・・・・・・/opencv-master/data/haarcascades/haarcascade_frontalface_default.xml"

# 通信用のソケットを作成
# ※アドレスファミリ：AF_INET（IPv4）、ソケットタイプ：SOCK_DGRAM（UDP）
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# リッスン状態にする
sock.bind(('', tello_port))

# データ受け取り用の関数
def run_udp_receiver():
    while True:
        try:
            data, server = sock.recvfrom(1024)
            print(data.decode(encoding="utf-8"))
            event.set()
        except Exception:
            print("Exit recv")
            break

# データ受け取り用のサブスレッドを開始
recv_thread = threading.Thread(target=run_udp_receiver)
recv_thread.daemon = True
recv_thread.start()

# Telloへの命令送信用の関数。（イベントをクリアしておかないとうまくうごかないです）
def send(command):
    msg = command.encode(encoding="utf-8")
    sent = sock.sendto(msg, tello_address)
    event.wait()
    event.clear()

# TelloのSDKモード起動、ビデオストリーム起動用の関数
def send_roop1():
    send("command")
    print("SDKモード開始")
    send("streamon")
    print("ストリーム開始")

# Telloの自動飛行用の関数
def send_roop2():
    time.sleep(3)
    send("takeoff")
    print("離陸")
    send("forward 250")
    print("250cm 前進")
    send("cw 90")
    print("90度　時計回りに回転")
    send("down 50")
    print("50cm　下降")
    send("cw 40")
    send("ccw 80")
    send("cw 40")
　　print("首振り運動")
    send("left 130")
    print("130cm 左移動")
    send("right 260")
    print("260cm 右移動")
    send("left 130")
    print("130cm 左移動")
    send("up 50")
    print("50cm 上昇")
    send("forward 300")
    print("300cm 前進")
    send("back 300")
    print("300cm 後退")
    send("down 50")
    print("50cm　下降")
    send("cw 90")
    print("90度　時計回りに回転")
    send("forward 250")
    print("250cm 前進")
    time.sleep(5)
    send("land")
    print("着陸")

# ビデオストリームをメインスレッドで動かしたかったので、飛行命令をサブスレッド化
send1_thread = threading.Thread(target=send_roop1)
send1_thread.start()

# ビデオストリームをメインスレッドで動かしたかったので、飛行命令をサブスレッド化
send2_thread = threading.Thread(target=send_roop2)
send2_thread.start()

# ビデオストリーム用の関数（OpencvのVideocapture関数のサイトを参考）
def video_capture():
    cap = None
    udp_video_address = 'udp://@' + VS_UDP_IP + ':' + str(VS_UDP_PORT)
    if cap is None:
        cap = cv2.VideoCapture(udp_video_address)
    if not cap.isOpened():
        cap.open(udp_video_address)
    while True:
        try:
            ret, frame = cap.read()
            # 顔認識の分類機をセット
            cascade = cv2.CascadeClassifier(cascade_path)
            facerect = cascade.detectMultiScale(frame, scaleFactor=1.1, minNeighbors=1, minSize=(100, 100))
            if len(facerect) > 0:
                for rect in facerect:
                    # 矩形を描画する
                    cv2.rectangle(frame, tuple(rect[0:2]), tuple(rect[0:2]+rect[2:4]), (255, 255, 255), 3)
            cv2.imshow('streaming', frame)
            for i in range(5):
                img = cap.read()
            if cv2.waitKey(100) & 0xFF == ord('q'):
                break
        except Exception:
            print("Exit capture")
            break
    send("streamoff")
    print("streamoff")
    cap.release()
    cv2.destroyAllWindows()
    sock.close()
    print("End")

print("start capture")
video_capture()

その他、参考にさせて頂いたサイト

http://kodamap.hatenablog.com/entry/2018/12/08/000000
https://qiita.com/mozzio369/items/8e0fb12dc30c493f5cc4
https://tellopilots.com/threads/djitellopy-a-new-python-wrapper-for-tello.2518/
https://github.com/damiafuentes/DJITelloPy