トイドローンTelloをPython3で制御する

今回は,トイドローンTelloをpythonを使って制御します！

目標

・Python3でterminalからドローンを制御できるようにする
ex) takeoffと打ったら離陸、landと打ったら着陸

動作環境

Mac Mojave 10.14.5
Python ver. 3.7.0

前提

・アプリからTelloを動かしたことはある
・pcにPython3をインストール済み

1.サンプルコードのダウンロード

今回は、Ryze Technology社が公式に出しているdji SDKを参考にして実装することにします。以下からコードをダウンロードしてください。
URL: https://github.com/dji-sdk/Tello-Python
または、ターミナルで以下を実行してください。

git clone https://github.com/dji-sdk/Tello-Python.git

2.Single_Tello_TestをPython3に対応させる

このSDKはPython2系で書かれています。
2系はサポートもなくなるのでそろそろ卒業したい、、ということで3系に対応させるように書き換えていきます。
まずは、Single_Tello_Testのディレクトリーに移動してください。
以下の3つのファイルを書き換えます。(名前が同じだと分からなくなるので変えてます)

書き換えるファイル -> 書き換えた後

・tello.py -> tello3.py
・stats.py -> stats3.py
・tello_test.py -> tello_test3.py

以下が書き換えた後のコードになります。

tello3.py

import socket
import threading
import time
from stats3 import Stats

class Tello:
    def __init__(self):
        self.local_ip = ''
        self.local_port = 8889
        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # socket for sending cmd
        self.socket.bind((self.local_ip, self.local_port))

        # thread for receiving cmd ack
        self.receive_thread = threading.Thread(target=self._receive_thread)
        self.receive_thread.daemon = True
        self.receive_thread.start()

        self.tello_ip = '192.168.10.1'
        self.tello_port = 8889
        self.tello_adderss = (self.tello_ip, self.tello_port)
        self.log = []

        self.MAX_TIME_OUT = 15.0

    def send_command(self, command):
        """
        Send a command to the ip address. Will be blocked until
        the last command receives an 'OK'.
        If the command fails (either b/c time out or error),
        will try to resend the command
        :param command: (str) the command to send
        :param ip: (str) the ip of Tello
        :return: The latest command response
        """
        self.log.append(Stats(command, len(self.log)))

        self.socket.sendto(command.encode('utf-8'), self.tello_adderss)
        print ('sending command: %s to %s' % (command, self.tello_ip))

        start = time.time()
        while not self.log[-1].got_response():
            now = time.time()
            diff = now - start
            if diff > self.MAX_TIME_OUT:
                print( 'Max timeout exceeded... command %s' % command)
                # TODO: is timeout considered failure or next command still get executed
                # now, next one got executed
                return
        print ('Done!!! sent command: %s to %s' % (command, self.tello_ip))

    def _receive_thread(self):
        """Listen to responses from the Tello.

        Runs as a thread, sets self.response to whatever the Tello last returned.

        """
        while True:
            try:
                self.response, ip = self.socket.recvfrom(1024)
                print('from %s: %s' % (ip, self.response))

                self.log[-1].add_response(self.response)
            except socket.error as exc: #change
                print ("Caught exception socket.error : %s" % exc)

    def on_close(self):
        pass
        #for ip in self.tello_ip_list:
        self.socket.sendto('land'.encode('utf-8'), (ip, 8889))
        self.socket.close()

    def get_log(self):
        return self.log

stats3.py

from datetime import datetime

class Stats:
    def __init__(self, command, id):
        self.command = command
        self.response = None
        self.id = id

        self.start_time = datetime.now()
        self.end_time = None
        self.duration = None

    def add_response(self, response):
        self.response = response
        self.end_time = datetime.now()
        self.duration = self.get_duration()
        # self.print_stats()

    def get_duration(self):
        diff = self.end_time - self.start_time
        return diff.total_seconds()

    def print_stats(self):
        print('\nid: %s' % self.id)
        print('command: %s' % self.command)
        print('response: %s' % self.response)
        print('start time: %s' % self.start_time)
        print('end_time: %s' % self.end_time)
        print('duration: %s\n' % self.duration)

    def got_response(self):
        if self.response is None:
            return False
        else:
            return True

    def return_stats(self):
        str = ''
        str +=  '\nid: %s\n' % self.id
        str += 'command: %s\n' % self.command
        str += 'response: %s\n' % self.response
        str += 'start time: %s\n' % self.start_time
        str += 'end_time: %s\n' % self.end_time
        str += 'duration: %s\n' % self.duration
        return str

tello_test3.py

#tello input test
#tello can be controlled by command line
#This code is originally tello_test.py
#it's modified to run with python3

from tello3 import Tello
import sys
from datetime import datetime
import time


start_time = str(datetime.now())
t1 = time.time()

#file_name = sys.argv[1]

#f = open(file_name, "r")
#commands = f.readlines()

tello = Tello()
while True:

    command = input("write command:")

    if not command:
        break

    if command != '' and command != '\n':
        command = command.rstrip()

        if command.find('delay') != -1:
            sec = float(command.partition('delay')[2])
            print('delay %s' % sec)
            time.sleep(sec)
            pass
        else:
            tello.send_command(command)

    if 'end' in command:
        print('...')
        tello.on_close()
        break

    if time.time()-t1 > 20: #max 20 secs
        tello.send_command('land')
        tello.on_close() #land and kill socket connection
        break

log = tello.get_log()

out = open('log/' + start_time + '.txt', 'w')
for stat in log:
    stat.print_stats()
    str = stat.return_stats()
    out.write(str)

大きな変更点

tello3.pyとstats3.pyについて

大きな変更はありませんが、
・printに()をつけた
・except socket.error, exc: -> except socket.error as exc: に変更

tello_test3.pyについて

こちらは結構変更しています。
もともと、tello_test.pyはcommand.txtにコマンドを入れて、
そのcommandを順にtelloに送信するという形になっています。
これでは、もともと決めた順でしかtelloを動かせません。
そのままのtello_test.pyを試したい方は以下の記事を参考にしてやってみてください。
URL: https://qiita.com/hsgucci/items/a199e021bb55572bb43d

その場でターミナルにコマンドを打って動かしたいのでinput()関数でcommandを入力してtelloに送信する仕様に変えました。
また、細かい部分でちょっと変更していますのでそこはコードを見てください。

3.いよいよドローンをpython3で制御

ひとまず、上のコードをそのままコピペしてファイルを作って以下のようにterminalで実行してみてください。

$ python tello_test3.py

すると、以下のようになります。

$ python tello_test3.py
write command:

最初にcommandと打ってください。
これをtelloに送信することでtelloが命令を受け取る準備が整います。
すると、以下のようになります。これでいよいよtelloを動かせます。

write command:command
sending command: command to 192.168.10.1
from ('192.168.10.1', 8889): b'OK'
Done!!! sent command: command to 192.168.10.1
write command:

ひとまず、離陸と着陸です。命令は以下のようになります。
離陸: takeoff
着陸: land

write command:command
sending command: command to 192.168.10.1
from ('192.168.10.1', 8889): b'OK'
Done!!! sent command: command to 192.168.10.1
write command:takeoff
sending command: takeoff to 192.168.10.1
from ('192.168.10.1', 8889): b'OK'
Done!!! sent command: takeoff to 192.168.10.1
write command:land
sending command: land to 192.168.10.1
from ('192.168.10.1', 8889): b'OK'
Done!!! sent command: land to 192.168.10.1

これでtelloが離陸して着陸すれば成功です！！
あとは、同じ流れでtakeoffとlandの間にたくさんの命令をしてやれば自由に動かすことができます。
この公式ガイドにcommandがたくさん乗っているので参考にしてください。

まとめ

ドローンのtelloをpython3で制御ができるようになりました。
今回はterminalから命令を入力しましたが、これを音声入力に変えたりするだけでもっと面白いものができるかも？
では！

参考

参考にした記事は以下です。
ソケット通信: https://qiita.com/__init__/items/5c89fa5b37b8c5ed32a4
Telloサンプル動かす: https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c
Telloで自動追尾する: https://qiita.com/mozzio369/items/1f80103339faaedc6be3