【Tello】トイ・ドローンで遊んでみた♪～キーボードのキーを使ってTelloを制御

これまでcv2.meanshiftを利用してTrackingを試みてきたが、それなりに苦労したので、第五夜は「キーボードのキーを使ってTelloを制御」をまとめておこうと思う。
そもそもTrackingのための制御コマンドの確認と物体検出搭載の前段階としての意味でも重要なプロセスである。
###一応の目標
①とにかく飛行させる
②GamePadで飛ばす
③PCで飛ばす
④Pythonで飛ばす
⑤画像入力する
⑥物体検出搭載
⑦トラッキングする
⑧物体を探して見つける
⑨...
今回やったこと
⑨キーボードのキーを使ってTelloを制御
###コードは以下に置いた
Tello/simple_takeoff_KeyTelloCamera.py
###⑨キーボードのキーを使ってTelloを制御
当初、k = cv2.waitKey(1)&0xffが結構問題でした。
・これって、画像入力と関係しているらしく、cv2.imshow("org",image)がないと滑ってキー入力を拾ってくれない（原因不明で不確実）
・６４ビットでは&0xffをつけないとミスることがある
　実際Printして確かめてみると、初期値が-1が返ってくるのが255に変換される
　※＆取っているからだろうけど、-1以外は(uhjnfqについては)数値は変わらなかった。
　　関連してord(q)=117．。。変換関数
・実は最初k = cv2.waitKey(1)&0xffのkがだめで、keyと変更したほど。。。単なる杞憂でしたが。。一部残しました（笑）原因は上の滑りだったみたいです。
以下コードを見ていこうと思います。
１．importは以下のとおり

from time import sleep　#sleep(5)などで待ちをしてる
import tellopy          #Tello制御
import cv2              #画像
import av               #画像コンテナで利用
import numpy            #numpy.array(frame.to_image())で利用

２．Tello状態取得のdrone.subscribe(drone.EVENT_FLIGHT_DATA, handler)のおまじない

def handler(event, sender, data, **args):
    drone = sender
    if event is drone.EVENT_FLIGHT_DATA:
        print(data)

３．最初のおまじない

    #cap = cv2.VideoCapture(0) #PCカメラでドローン撮影の時利用
    drone = tellopy.Tello()    #Tellopy利用してオブジェクト化
    #drone.subscribe(drone.EVENT_FLIGHT_DATA, handler) #Telloの情報取得をする場合に利用だが、ログが多すぎる上、これやると不安定になるので削除

    drone.connect()  #Tello接続
    drone.wait_for_connection(60.0)  #接続まで待つ
    container = av.open(drone.get_video_stream())  #Videoを開く
    x = 200
    y = 200
    w = 224
    h = 224
    track_window = (x, y, w, h)  #動画の表示Window
    frame_skip=300  #動画接続前のframeを飛ばす（300より少なくてもいいかも）

４．いよいよ動画の助走してdrone.takeoff
ここで#cv2.destroyWindow("org")をコメント外してもいいが連続性から外さない方が滑らか

    for frame in container.decode(video=0):
        if 0 < frame_skip:
            frame_skip = frame_skip - 1
            continue
        image = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
        cv2.imshow("org",image)
        k = cv2.waitKey(1)&0xff
        if k == ord('q'):
            #cv2.destroyWindow("org")
            break
            
    drone.takeoff()

５．制御本体
参考のTellopyコードを見ると引数の意味が分かります。
【参考】
・TelloPy / tellopy / _internal / tello.py

    while True:
        for frame in container.decode(video=0):
            if 0 < frame_skip:
                frame_skip = frame_skip - 1
                continue
            
            image = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
        
            cv2.imshow("org",image)
            key = cv2.waitKey(1)&0xff
            print("key=",key,ord('q'))
        
            if key == ord('n'):  #n
                drone.down(10)
                sleep(5)
            elif key==ord('u'):  #117:  #u
                drone.up(10)
                sleep(5)
            elif key==ord('h'):  #104:  #h
                drone.left(3)
                sleep(1)
            elif key==ord('j'):  #106:  #j
                drone.right(3)
                sleep(1)
            elif key==ord('b'):  #backward
                drone.backward(3)
                sleep(1)
            elif key==ord('f'):  #forward
                drone.forward(3)
                sleep(1)
            elif key==ord('c'):  #clockwise
                drone.clockwise(10)
                sleep(1)    
            elif key==ord('q'):  #quit
                cv2.destroyAllWindows()   #残りは着陸なのでここで画像停止
                break            #forから脱出
            else:
                continue
        break                    #whileから脱出

６．最後は着陸

    drone.down(50)  #地面に近づいてから着陸
    sleep(5)        #5s待つ
    drone.land()    #着陸；手で受けられる
    sleep(5)        #ゆっくり
    drone.subscribe(drone.EVENT_FLIGHT_DATA, handler)  #TelloのBat状態など取得
    drone.quit()    #Quitする

###まとめ
・安定してTelloをキーボードのキーで制御でき、動画も綺麗に取れた（保存は未）

・いよいよ物体検出とTracking出来そう
###おまけ
以下がフライトのログ例

>python -m tellopy.examples.simple_takeoff_KeyTelloCamera
Tello: 16:00:20.503:  Info: start video thread
Tello: 16:00:20.503:  Info: send connection request (cmd="conn_req:9617")
Tello: 16:00:20.503:  Info: video receive buffer size = 524288
Tello: 16:00:20.503:  Info: state transit State::disconnected -> State::connecting
Tello: 16:00:20.550:  Info: connected. (port=9617)
Tello: 16:00:20.550:  Info: send_time (cmd=0x46 seq=0x01e4)
Tello: 16:00:20.550:  Info: state transit State::connecting -> State::connected
Tello: 16:00:20.550:  Info: get video stream
Tello: 16:00:20.550:  Info: start video (cmd=0x25 seq=0x01e4)
Tello: 16:00:20.596:  Info: recv: ack: cmd=0x34 seq=0x0000 cc 60 00 27 90 34 00 00 00 00 72 a5
Tello: 16:00:20.596:  Info: recv: ack: cmd=0x20 seq=0x0000 cc 60 00 27 b0 20 00 00 00 00 42 b9
Tello: 16:00:20.596:  Info: recv: ack: cmd=0x34 seq=0x0000 cc 60 00 27 90 34 00 00 00 00 72 a5
Tello: 16:00:20.612:  Info: recv: ack: cmd=0x20 seq=0x0000 cc 60 00 27 b0 20 00 00 00 00 42 b9
Tello: 16:00:23.533:  Info: video data 943053 bytes 457.1KB/sec
non-existing PPS 0 referenced
last message repeated 1 times
decode_slice_header error
no frame!
...
non-existing PPS 0 referenced
last message repeated 1 times
decode_slice_header error
no frame!
Tello: 16:00:25.548:  Info: video data 760813 bytes 368.7KB/sec
Tello: 16:00:27.550:  Info: video data 751217 bytes 366.6KB/sec
Tello: 16:00:29.560:  Info: video data 751063 bytes 364.9KB/sec
Tello: 16:00:30.723:  Info: VideoStream.seek(-1, 2)
Tello: 16:00:30.723:  Info: VideoStream.seek(-1, 2)
Tello: 16:00:31.562:  Info: video data 740076 bytes 361.0KB/sec
Tello: 16:00:33.564:  Info: video data 763612 bytes 372.5KB/sec
Tello: 16:00:35.565:  Info: video data 758225 bytes 370.0KB/sec
Tello: 16:00:37.580:  Info: video data 754914 bytes 366.0KB/sec
Tello: 16:00:39.595:  Info: video data 756810 bytes 366.7KB/sec
Tello: 16:00:41.049:  Info: set altitude limit 30m
Tello: 16:00:41.049:  Info: takeoff (cmd=0x54 seq=0x01e4)
Tello: 16:00:41.049:  Info: recv: ack: cmd=0x54 seq=0x0000 cc 60 00 27 b0 54 00 00 00 00 a1 81
key= 255 102
...
key= 255 102
key= 99 102
Tello: 16:00:52.519:  Info: clockwise(val=10)
key= 255 102
...
key= 255 102
key= 110 102
Tello: 16:01:18.322:  Info: down(val=10)
Tello: 16:01:19.877:  Info: video data 763545 bytes 369.9KB/sec
Tello: 16:01:21.892:  Info: video data 750969 bytes 363.9KB/sec
key= 255 102
...
key= 255 102
key= 102 102
Tello: 16:01:40.954:  Info: forward(val=3)
key= 255 102
...
key= 255 102
key= 113 102
Tello: 16:01:55.501:  Info: down(val=50)
Tello: 16:01:56.125:  Info: video data 751058 bytes 365.9KB/sec
Tello: 16:01:58.127:  Info: video data 762277 bytes 371.7KB/sec
Tello: 16:02:00.158:  Info: video data 752563 bytes 361.9KB/sec
Tello: 16:02:00.505:  Info: land (cmd=0x55 seq=0x01e4)
Tello: 16:02:00.536:  Info: recv: ack: cmd=0x55 seq=0x0000 cc 60 00 27 b0 55 00 00 00 00 e5 8a
Tello: 16:02:02.189:  Info: video data 763467 bytes 367.1KB/sec
Tello: 16:02:04.190:  Info: video data 746749 bytes 364.5KB/sec loss=3
Tello: 16:02:05.518:  Info: quit
Tello: 16:02:05.518:  Info: state transit State::connected -> State::quit
Tello: 16:02:05.518:  Info: VideoStream.handle_event(DISCONNECTED)
Tello: 16:02:05.518:  Info: exit from the video thread.
ALT:  0 | SPD:  0 | BAT: 35 | WIFI: 90 | CAM:  0 | MODE:  1
Tello: 16:02:05.597:  Info: exit from the recv thread.