PythonでTELLO-EDUの操作

# 環境

|機器・ソフト|バージョン|
|-|-|
|ノートPC|Windows10 Home|
|Python|3.7.0|
|opencv-python|3.4.2.17|

# 自作モジュール解説
自作モジュールは[GitHub](https://github.com/ushigai/tellopy)においてあります。

## 各種セットアップ
セットアップは`setup_tello()`で行っています。~~`__init__()`でもよかったのですが……~~



## SDKコマンドやりとり
TelloへのSDKコマンドの送信またそれに対する応答は113行目にある`go_tello()`で制御しています。
まず`go_tello()`に引数でコマンドを受け取りコマンドを`sock.sendto()`で送ります。その後154行目の`while`ループに突入し、`self.RECIEVE_DATA`が`None`の間すなわち、ドローンから応答が返って来るまで処理待ち状態にします。ドローンからの応答は、

**実行成功：** OK
**実行失敗：** error (エラー文が続く場合がある)

のような形式で返ってきます。
ドローンの応答が返ってきたら、クールダウンを行います。TELLO-EDUはコマンドとコマンドの間を短くすると`error not joystick`なるエラーが返ってきます。なので`threading.Timer()`を用いて処理待ち状態にします。ちなみに`time.sleep()`だとなぜかうまくいきませんでした。

```python
    def __receive_thread__(self):
        """
        Telloからの応答監視スレッド
        最後の応答がreceive_dataに格納される
        """
        while self._receive_going:
            try:
                self.RECEIVE_DATA, ip = self.sock.recvfrom(3000)
            except:
                print(traceback.format_exc())
```

```python
    def go_tello(self, S, interval=1.0):
        """
        S : コマンド(大文字小文字どちらでもいい)
        interval : 入力コマンド間の遅延

        command
        takeoff
        land
        up[down, left, right, forward, back] "Value" : "Value" = 20-500
        cw[ccw] "Value" : "Value" = 1-360
        flip "Direction" : "Direction" = l(left), r(right), f(forward), b(back)
        """

        # command, emergency は処理上ない
        _CONTROL_COMMANDS_ = ["takeoff", "land", "streamon", "streamoff", "up", "down", "left",
                              "right", "forward", "back", "cw", "ccw", "flip", "go", "stop",
                              "curve", "jump"]

        _SET_COMMANDS_ = ["speed", "rc", "wifi",
                          "mon", "moff", "mdirection", "ap"]

        _READ_COMMANDS_ = ["speed?", "battery?", "time?", "wifi?", "height?", "temp?", "attitude?"
                           "baro?", "acceleration?", "tof?"]

        _ALL_COMMANDS_ = _CONTROL_COMMANDS_ + _SET_COMMANDS_ + _READ_COMMANDS_

        if isinstance(S, str):
            S = S.lower()
            self.DATA_FLAG = False
            sent = self.sock.sendto(S.encode("utf-8"),
                                    self.tello_address)  # コマンド送信
            print(S, ">> Tello")

            # タイマースレッドで処理待ち状態に
            # time.sleepとかだとうまくいかない
            timer = threading.Timer(interval, self.set_flag_TRUE)
            timer.start()  # タイマースレッドスタート

            S = S.split()
            S = S[0]

            while self.RECEIVE_DATA is None:
                if S in _ALL_COMMANDS_:
                    continue
                if self.DATA_FLAG:
                    break
            timer.cancel()  # タイマースレッドストップ

            if self.RECEIVE_DATA is None:
                response = "NULL"
            else:
                response = self.RECEIVE_DATA.decode("utf-8")
            self.RECEIVE_DATA = None
            print(response + " << Tello")
        else:
            raise TypeError
        self.DATA_FLAG = False
        timer = threading.Timer(interval, self.set_flag_TRUE)
        timer.start()  # タイマースレッドスタート
        while True:
            if self.DATA_FLAG:
                break
```

## カメラ制御
Telloのカメラ制御にはpythonの`opencv-python`モジュールを使用しました。(バージョンは最新のものではない)

カメラ制御の挙動としては、Telloが他のSDKコマンドを実行している最中でもPCでTelloの映像をリアルタイムで表示できるようにしました。再び`threading`を使用します。
`VIDEO_FRAME`にフレームデータを格納し、それを`cv2.imshow()`で表示しています。`cv2.waitKey()`はキー入力を引数に与えた時間（単位はミリ秒）だけ待つ関数で一見必要ないように見えますが、これがないと`cv2.imshow()`ができないので入れています。

```python
def __video_thread__(self):
    """
    ビデオスレッド
    カメラのフレームデータをself.VIDEO_FRAMEに格納する
    """
    self.VIDEO_FRAME = None
    self.rect = []
    while self._video_going:
        ret, self.VIDEO_FRAME = self.cap.read()
        if len(self.rect) == 1: # 顔が検出された場合、四角で囲む
            x, y, w, h = self.rect[0]
            cv2.rectangle(self.VIDEO_FRAME, tuple([x, y]), tuple([x + w, y + h]), (0, 0, 255), thickness=3)
        cv2.imshow("frame_data", self.VIDEO_FRAME)
        _ = cv2.waitKey(1)
    #cv2.destroyAllWindows()
```

また`take_picture()`という写真撮影を行う関数も作成しました。引数`filename`は撮影した画像データを保存するときのファイル名で、特に指定がなければ`frame_filename`が適当にナンバリングしてくれます。

```python
def take_picture(self, filename=None):
    """
    Telloのカメラの最新フレームを保存する
    保存先はself.frame_save_path
    保存名はself.frame_filename
    """
    if filename is None:  # ユーザがファイル名を指定しなかった場合ファイル名をナンバリング
        filename = self.frame_filename
        self.frame_filename = str(int(self.frame_filename))
        self.frame_filename = str(eval(self.frame_filename + " + 1"))
        self.frame_filename = self.frame_filename.zfill(6)  # 整形のためゼロ埋め
        filename = self.frame_filename
    cv2.imwrite(self.frame_save_path + filename + ".jpg", self.VIDEO_FRAME)
    print(f"Saved the photo as filename:{filename} << Tello")
```

## 顔認識
顔認識にはOpenCVに付属していたカスケード分類器を使用しました。付属の分類器の顔認識の（大雑把な）精度は以下の通りでした。

|分類器|精度|
|-|-|
|haarcascade_frontalcatface|良くなかった|
|haarcascade_frontalcatface_extended|良くなかった|
|haarcascade_frontalface_alt|多少認識する|
|haarcascade_frontalface_alt_tree|良くなかった|
|haarcascade_frontalface_alt2|良かった|
|haarcascade_frontalface_default|良かった|

`pytello.py`にある通り私が実装した際には`haarcascade_frontalface_default.xml`を使用しました（`alt2`でもいい感じかも）。

顔認識は294行目の`face_detection()`にあります。スレッドで走らせている`__video_thread__()`の`VIDEO_FRAME`から`opencv`のフレームデータを読み取り、グレースケールにして`HaarCascade`で殴っています。



```python
def face_detection(self):
    """
    現在のフレームデータ(self.VIDEO_FRAME)にて人の顔があるかどうか識別
    肝心な顔認識の部分はHaar Cascadeで殴ってる
    返し値は認識した顔の[(左上のx座標), (左上のy座標), (横幅), (縦線)]が、
    認識した顔の数だけlistで返される
    """
    self.rect = []
    cv2.imwrite("./_camera_frame/temp.jpg", self.VIDEO_FRAME)  # いったん空打
    src = cv2.imread("./_camera_frame/temp.jpg", 0)
    gray = cv2.cvtColor(src, cv2.cv2.COLOR_BAYER_BG2GRAY)
    cascade = cv2.CascadeClassifier(self.cascade_path)
    self.rect = cascade.detectMultiScale(gray, minSize=(100, 100))
    return self.rect
```


## 色認識
色認識はHSV閾値を用いて行いました。
`color_detection()`に引数`upper`, `lower`として閾値を入力します。引数を受け取った関数は`VIDEO_FRAME`からフレームデータを取得し`cv2.cvtColor()`でHSVに変換して、指定色範囲以外をマスクします。マスク結果は`find_specific_color()`に引数として渡されます。

`find_specific_color()`は特定の色の座標を返すプログラムで、[TonyMoooriさんのGiuHub](https://gist.github.com/TonyMooori/4cc29c94f7bdbade6ff6102fef45232e)から使用しました。

```python
def color_detection(self, lower, upper):
    """
    閾値に対して色検出を行いその座標を返す。
    lower, upperにそれぞれ[h, s, v]を指定してぶち込む。
    """
    cv2.imwrite("./_camera_frame/temp.jpg", self.VIDEO_FRAME)
    img = cv2.imread("./_camera_frame/temp.jpg")

    # HSVに変換
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

    # numpyのarrayじゃなきゃだめじゃね？
    lower = np.array(lower) 
    upper = np.array(upper)

    # 指定色範囲以外をマスク
    img_mask = cv2.inRange(hsv, lower, upper)
    res = cv2.bitwise_and(img, img, mask=img_mask)
    cv2.imwrite("./_camera_frame/teemp.jpg", res)  # マスクした結果を保存

    # 認識対象の色のx座標, y座標, 面積を返す
    return self.find_specific_color(img, 0, lower, upper)

```

```python
def find_specific_color(self, frame, AREA_RATIO_THRESHOLD, LOW_COLOR, HIGH_COLOR): # https://gist.github.com/TonyMooori/4cc29c94f7bdbade6ff6102fef45232e 
    """
    指定した範囲の色の物体の座標を取得する関数
    frame: 画像
    AREA_RATIO_THRESHOLD: area_ratio未満の塊は無視する
    LOW_COLOR: 抽出する色の下限(h,s,v)
    HIGH_COLOR: 抽出する色の上限(h,s,v)
    """
    h, w, c = frame.shape
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    ex_img = cv2.inRange(hsv, LOW_COLOR, HIGH_COLOR)

    # 面積を計算
    _, contours, hierarchy = cv2.findContours(
        ex_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    areas = np.array(list(map(cv2.contourArea, contours)))

    if len(areas) == 0 or np.max(areas) / (h * w) * 10000 < AREA_RATIO_THRESHOLD:
        # 見つからなかったらNoneを返す
        return None, None, None
    else:
        # 面積が最大の塊の重心を計算し返す
        max_idx = np.argmax(areas)
        max_area = areas[max_idx]
        result = cv2.moments(contours[max_idx])
        x = int(result["m10"]/result["m00"])
        y = int(result["m01"]/result["m00"])
        return [x, y, np.max(areas) / (h * w) * 10000]
```