TelloEduで2機編隊飛行 〜 両機が反対方向の行動を取るようプログラミングしてみる（成功）

__過去2本の記事__の続編に続いて、いよいよ2機のドローンに対して、Macbook PCから同時に操作コマンドを送信してみます。2機とも、離陸から着陸まで、意図した通りに動作しました。

• 前回の記事
• 前々回の記事
• 最初の記事

行ったこと

Wifiルータ経由で、Macbook PCのTerminalに打ち込んだ操縦コマンドを、同時に2機のTelloEduドローンに送信して、操作に成功しました。

Terminalに打ち込む操縦コマンドは、1機目に対するコマンド指令だけです。
2機目のドローンは、1機目に与えられた指令と、一部反対の行動を自動で実施します。

2機目のドローンは、次の__if ~ then・・・ ロジックに従って、「1機目に与えられた指示の内容」をもとに、自分がとるべき行動を自律的に判断__します。

以下、__msg__には、__1機目のドローンが、ユーザからコンソール入力で受け取った飛行指令コマンド__が格納されています。

判断ロジック＠2機目のドローン

if "cw"  in msg:
	msg2 = 'ccw ' + msg[3:]
elif "up" in msg:
	msg2 = "down " + msg[3:]
elif "down"  in msg:
	msg2 = "up " + msg[5:]
elif "forward" in msg:
	msg2 = "back " + msg[8:]
elif "back" in msg:
    msg2 = "forward " + msg[5:]
else:
	msg2 = msg

実行コード

Terminal

% python3 two_drone_remote_ctl2.py -drone1_ip 192.168.0.9 -drone2_ip 192.168.0.11

実行画面

Terminal

electron@diynoMacBook-Pro TelloEduSwarm % python3 two_drone_remote_ctl2.py -drone1_ip 192.168.0.9 -drone2_ip 192.168.0.11
1機目のドローンに接続します。('192.168.0.9', 8889)
2機目のドローンに接続します。('192.168.0.11', 8889)
Tello1号機に送る指示コマンドを入力してください。：  takeoff
1号機の実施行動：　takeoff
2号機の実施行動：　takeoff
Tello1号機に送る指示コマンドを入力してください。：  cw 720
1号機の実施行動：　cw 720
2号機の実施行動：　ccw 720
Tello1号機に送る指示コマンドを入力してください。：  up 50
1号機の実施行動：　up 50
2号機の実施行動：　down 50
dowTello1号機に送る指示コマンドを入力してください。：  down 50
1号機の実施行動：　down 50
2号機の実施行動：　up 50
Tello1号機に送る指示コマンドを入力してください。：  cw 180
1号機の実施行動：　cw 180
2号機の実施行動：　ccw 180
Tello1号機に送る指示コマンドを入力してください。：  forward 50
1号機の実施行動：　forward 50
2号機の実施行動：　back 50
Tello1号機に送る指示コマンドを入力してください。：  back 50
1号機の実施行動：　back 50
2号機の実施行動：　forward 50
Tello1号機に送る指示コマンドを入力してください。：  land
1号機の実施行動：　land
2号機の実施行動：　land
Tello1号機に送る指示コマンドを入力してください。：  ^Z
zsh: suspended  python3 two_drone_remote_ctl2.py -drone1_ip 192.168.0.9 -drone2_ip 
electron@diynoMacBook-Pro TelloEduSwarm % 

electron@diynoMacBook-Pro TelloEduSwarm % python3 two_drone_remote_ctl2.py -drone1_ip 192.168.0.9 -drone2_ip 192.168.0.11
1機目のドローンに接続します。('192.168.0.9', 8889)
2機目のドローンに接続します。('192.168.0.11', 8889)
Tello1号機に送る指示コマンドを入力してください。：  takeoff
1号機の実施行動：　takeoff
2号機の実施行動：　takeoff
Tello1号機に送る指示コマンドを入力してください。：  up
1号機の実施行動：　up
2号機の実施行動：　down 
Tello1号機に送る指示コマンドを入力してください。：  down
1号機の実施行動：　down
2号機の実施行動：　up 
Tello1号機に送る指示コマンドを入力してください。：  up 50
1号機の実施行動：　up 50
2号機の実施行動：　down 50
Tello1号機に送る指示コマンドを入力してください。：  down 50
1号機の実施行動：　down 50
2号機の実施行動：　up 50
Tello1号機に送る指示コマンドを入力してください。：  up 100
1号機の実施行動：　up 100
2号機の実施行動：　down 100
Tello1号機に送る指示コマンドを入力してください。：  down 100
1号機の実施行動：　down 100
2号機の実施行動：　up 100
Tello1号機に送る指示コマンドを入力してください。：  land 
1号機の実施行動：　land
2号機の実施行動：　land
Tello1号機に送る指示コマンドを入力してください。：  ^Z
zsh: suspended  python3 two_drone_remote_ctl2.py -drone1_ip 192.168.0.9 -drone2_ip 
electron@diynoMacBook-Pro TelloEduSwarm % 

実行したスクリプト

two_drone_remote_ctl2.py

import socket, argparse, time    

# 各機体のIPアドレスを、コマンドライン引数から受け取る
parser = argparse.ArgumentParser(description='')  
parser.add_argument('-drone1_ip')  
parser.add_argument('-drone2_ip')  

args = parser.parse_args() 
fst_drone_ip = args.drone1_ip
snd_drone_ip = args.drone2_ip

# print("IPアドレス {0} 番のTelloEduドローンに操作コマンドを送ります。".format(tello_ip))

# TelloEduのポート番号は全機体、同じ定数文字列
tello_port = 8889
fst_drone_address = (fst_drone_ip, tello_port)
snd_drone_address = (snd_drone_ip, tello_port)

print("1機目のドローンに接続します。" + str(fst_drone_address))
print("2機目のドローンに接続します。" + str(snd_drone_address))

socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# 両機にcommand指令を伝達
socket.sendto('command'.encode('utf-8'), fst_drone_address)
socket.sendto('command'.encode('utf-8'), snd_drone_address )

# 前の動作が終わるまで、sleepで待機
time.sleep(3)

while True: 
    # 最初に入力が必要な"command"命令は、ユーザが打ち込まなくて良いようにスクリプト側でTelloに送出する

    # "takeoff"以降の命令をユーザ入力から受け取る
    try:
        msg = input("Tello1号機に送る指示コマンドを入力してください。：  ")
        if not msg:
            break  
        
        if "cw"  in msg:
        	msg2 = 'ccw ' + msg[3:]
        elif "up" in msg:
        	msg2 = "down " + msg[3:]
        elif "down"  in msg:
        	msg2 = "up " + msg[5:]
        elif "forward" in msg:
        	msg2 = "back " + msg[8:]
        elif "back" in msg:
        	msg2 = "forward " + msg[5:]
        else:
        	msg2 = msg
        
        print("1号機の実施行動：　" + str(msg))
        print("2号機の実施行動：　" + str(msg2))
        socket.sendto(msg.encode(encoding="utf-8") , fst_drone_address)
        socket.sendto(msg2.encode(encoding="utf-8") , snd_drone_address)
        time.sleep(3)
    except KeyboardInterrupt:
        sock.close()  
        break

今後の抱負

2機のTelloEduから送信される画像データを、Macbook側で受信して、ウインドウに表示させたい。
Tello-PythonやTelloPyを使わずに、以下の記事にあるように、自分でコードを書いて動かしたい。

• Tello(ドローン)を使った顔認識自動追尾システム

• （Qiita）[Python]telloEduで編隊飛行を行った際のカメラ映像の取得方法を教えてください。

• （teratail）[Python]telloEduで編隊飛行を行った際のカメラ映像の取得方法を教えてください。

• TELLO SDK 2.0 User Guide

Receive Tello Video Stream

• Tello で Object tracking

Telloから Video Stream を受信するには "streamon" コマンドを送信した後、 UDP port 11111 で ストリーミングを待ち受ける必要があります。

UDP Socketを作って bind する必要があると思ったのですが、明示的な bind の記述は必要なく以下のコードで動作します。 cv2.VideoCapture('udp://127.0.0.1:11111') が実行されるタイミングで UDP 0.0.0.0 :11111 でリッスンします。OpenCV の ドキュメント や ソース を 見ても All Address でリッスンする部分は確認できず、今でもしっかり理解できていません。

• ニート翔ぶ～C#でドローンを飛ばす～

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

Tello IP: 192.168.10.1 ->> PC/Mac/Mobile UDP Server:0.0.0.0 UDP PORT:11111
　注意４：PC、Mac、またはモバイルデバイスにUDPサーバーをセットアップし、UDP PORT 11111を介してIP 0.0.0.0からメッセージを受信します。
　注意５：もし実行していないなら注意２を実行してください。その後、UDP PORT 8889を介して「streamon」コマンドをTelloに送信してストリーミングを開始します。