ドローン・プログラミングでTelloを無人自動飛行してみた（Python3）

__前回の記事__の続編です。

###全体の構成

前回は、MacbookのTerminalに、人間が手入力で"takeoff", "forward 100", ・・・, "land"と、Tello APIで提供されているコマンドをひとつひとつ入力していました。

MacbookからWifi通信でTelloに命令を送れるようになったものの、これだと、同じくWifi通信で、iPhone/ iPadの画面に表示されたバーチャル・ジョイスティックで操作した方が、操作しやすいです。

そこで今回は、一連の飛行動作を、あらかじめスクリプトに記述（列挙）して、人間は手放しでTelloが飛ぶさまを眺めていられるように、Pythonスクリプトを書き換えてみました。

プログラムの中では、__for文__で、繰り返し処理を行うようにしました。（時計回り45度回転を8回繰り返す）。

今後、__if文__と、__一般物体認識（OpenCVの機能）__を組み合わせて、次のようなことにも挑戦してみたいと思います。

• Telloに内蔵されているカメラに、"椅子"が写ったら、そこに着地（"land"）させる。
• Tello内蔵カメラに、人が写ったら、衝突するリスクを減らすために、300上昇する（"up 300"）

今回、取り組む内容を次に述べます。
試験飛行を行った結果、意図した通りに動作することを確認しています。

###プログラムした自動飛行経路

###実行したスクリプト

# Tello Python3
import threading 
import socket
import sys
import time

host = ''
port = 9000
locaddr = (host,port) 

# Create a UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
tello_address = ('192.168.10.1', 8889)

sock.bind(locaddr)

def recv():
    count = 0
    while True: 
        try:
            data, server = sock.recvfrom(1518)
        except Exception:
            print ('\nExit . . .\n')
            break


def takeoff():
    sent = sock.sendto("takeoff".encode(encoding="utf-8"), tello_address)


def land():
    sent = sock.sendto("land".encode(encoding="utf-8"), tello_address)


def forward(n):
    order = "forwarding " + str(int(n))
    sent = sock.sendto(order.encode(encoding="utf-8"), tello_address)


def up(n):
    order = "up " + str(int(n))
    sent = sock.sendto(order.encode(encoding="utf-8"), tello_address)

def clockwise_45degree_8times_repeating():
    i = 0
    for i in range(8):
        if i == 0:
            print("Clockwise 45degree Rotating 1st turn")
        elif i == 1:
            print("Clockwise 45degree Rotating 2nd  turn")
        else:
            print("Clockwise 45degree Rotating " + str(i+1) + "-th turn")           
        sent = sock.sendto("cw 45".encode(encoding="utf-8"), tello_address)
        time.sleep(3)
        i +=1

#recvThread create
recvThread = threading.Thread(target=recv)
recvThread.start()

print("start")
sent = sock.sendto("command".encode(encoding="utf-8"), tello_address)
time.sleep(8)

try:
    print("Taking off")
    takeoff()
    time.sleep(3)
    up(100)
    time.sleep(3)
    clockwise_45degree_8times_repeating()

    print("Ascending 100cm")
    up(100)
    time.sleep(3)
    print("forwarding 150cm")
    forward(150)
    time.sleep(3)    
    clockwise_45degree_8times_repeating()

    print("Landing down")
    land()
    time.sleep(3)
    sock.close()     
  
except KeyboardInterrupt:
    print("forced shutdown button pressed. Landing down.")
    land()
    time.sleep(8)
    sock.close() 

##実行結果（１）

スクリプトに記述した動作が、最初から最後まで全部実行できた。

（１） Rotating 1st turn〜Rotating 8-th turn： 時計回り45度の回転を8回繰り返し、360度回転
（２） 100cm上昇後
（３） 150cm前進
（４） Rotating 1st turn〜Rotating 8-th turn： 時計回り45度の回転を8回繰り返し、360度回転

Terminal

electron@diynoMacBook-Pro Tello % python3 TelloScript.py          
start
Taking off
Clockwise 45degree Rotating 1st turn
Clockwise 45degree Rotating 2nd  turn
Clockwise 45degree Rotating 3-th turn
Clockwise 45degree Rotating 4-th turn
Clockwise 45degree Rotating 5-th turn
Clockwise 45degree Rotating 6-th turn
Clockwise 45degree Rotating 7-th turn
Clockwise 45degree Rotating 8-th turn
Ascending 100cm
forwarding 150cm
Clockwise 45degree Rotating 1st turn
Clockwise 45degree Rotating 2nd  turn
Clockwise 45degree Rotating 3-th turn
Clockwise 45degree Rotating 4-th turn
Clockwise 45degree Rotating 5-th turn
Clockwise 45degree Rotating 6-th turn
Clockwise 45degree Rotating 7-th turn
Clockwise 45degree Rotating 8-th turn
Landing down

Exit . . .

electron@diynoMacBook-Pro Tello % 

##実行結果（２）

スクリプト実行により、Telloが離陸した後に、Macbookキーボード側でCtrl-Z（強制終了）を叩いた。スクリプトに記述した通り、Telloは数秒後に、自動着陸した。

Terminal

electron@diynoMacBook-Pro Tello % python3 TelloScript.py
start
Taking off
Clockwise 45degree Rotating 1st turn
Clockwise 45degree Rotating 2nd  turn
Clockwise 45degree Rotating 3-th turn
Clockwise 45degree Rotating 4-th turn
^Z
zsh: suspended  python3 TelloScript.py

electron@diynoMacBook-Pro Tello %