Tello Eduをpythonで動かそう！（SDK2対応）

Tello EduのSDK2を使ったプログラミング教育を行うための環境開発

概要

ミニ（ホビー）ドローンを使って小学生高学年から高等学校の児童生徒がプログラミングを学べる教材を開発しています。下記の参考資料を参考に作成しています。ありがとうございます。

修正履歴：
（9月５日）MacOSでのzbarインストールについて
（9月9日）ソースコード　telloedu.py　の修正【バグ等】
（9月9日）ソースコード　tello.py　の修正【タイプミスの場合の対処】
（9月9日）操作方法に追記
（9月9日）Tello EduからのStatusデータの取得について追記
（9月10日）ソースコード　telloedu.py　および tello.py　の修正
（10月28日）ソースコードの見直し（パッケージ・モジュール化，　スレッドのThreadPoolExecutor対応）

実行環境

WindowsでもMacOSでも同じやり方で動いた！

• python3.7.4
• PyCharm 2019.2.1 (Community Edition)
• opencv 2
• pillow 6.1
• pyzbar 0.18

注意点

• pycharmの仮想環境の設定は，condaとする。

インストール

python

anacondaを用いてインストールする。

PyCharm 2019.2.1 (Community Edition)

プロジェクトを作る際に，仮想環境として[CONDA]を利用するように設定しておく

opencv

PyCharmで作成したプロジェクトの仮想環境が[anaconda]に出来ているので，その環境（Environments)を選択肢して，そこにPackageを追加する。

追加する場合は，Not Installedを選択して，opencvで検索を行う。
次の3つのパッケージをインストールする。 libopencv 3.4.2，opencv 3.4.2，　py-opencv 3.4.2をインストールする。

pillow

opencvと同様に，pillowを選択してインストール

pyzbar

pyzbarは，anacondaのライブラリとしてはない，また，anaconda cloudにおいても該当するものインストールする。ない場合は，PyCharmで該当するプロジェクト開き，コンソールから以下のコマンドを入れる。
ただし，MacOSの場合は，zbar本体のインストールが必要となる。

 pip install pyzbar

MacOSでzbarをインストールする方法

ソースコード

ファイルの設置

2つのファイルから構成しています。

• Tello Edu SDK2対応ライブラリ（パッケージ）[telloedu]
  • tello edu SDK2 コマンドモジュール[command.py]
  • tello edu SDK2 状態取得モジュール[status.py]
  • tello edu SDK2 映像系モジュール[streaming.py]
  • tello edu 共通関数モジュール[tellolib.py]
• メイン（プログラム）ファイル[tellomain.py]
• 演習用ファイル[ugoki.py]

その他に，

• QRコードを撮影した写真データが保存される[img]ディレクトリ，
• Tello Eduから送られてくる動画を保存する[mpg]ディレクトリ，
• Tello Eduから送られてくるStatusデータ（約5秒間隔）をCSVファイルとして保存する[data]ディレクトリ

も同一のディレクトリ内に作成しておいてください。

　.
　├── data/
　├── img/
　├── mpg/
　├── telloedu/
　│   ├── __init__.py
　│   ├── command.py
　│   ├── status.py
　│   ├── streaming.py
　│   └── tellolib.py
　├── tellomain.py
　└── ugoki.py

こんな感じです。

Tello Edu SDK2対応ライブラリ（パッケージ）

この部分まだバグがあるかもしれません。すべてのバグを取り切れているとは言えないと思っています。

command.py

#
# Tello Edu (SDK2.0 対応) Python3 Command Library
#

import socket
import time
import telloedu.streaming as streaming
from telloedu.tellolib import *

# Create a UDP socket
host = ''
port = 9000
tello_ip = '192.168.10.1'
tello_port = 8889
locaddr = (host, port)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
tello_address = (tello_ip, tello_port)
sock.bind(locaddr)


# command
def emergency():
	cmd = 'emergency'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while True:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
	streaming.video_recording = 0
	streaming.main_thread = False


def command():
	cmd = 'command'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		sock.close()


def takeoff():
	cmd = 'takeoff'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		sock.close()


def land():
	cmd = 'land'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		sock.close()


def stop():
	cmd = 'stop'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		sock.close()


def up(x):
	if type(x) is int:
		if (20 <= x) and (x <= 200):
			cmd = 'up ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def down(x):
	if type(x) is int:
		if 20 <= x <= 200:
			cmd = 'down ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def left(x):
	if type(x) is int:
		if 20 <= x <= 200:
			cmd = 'left ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def right(x):
	if type(x) is int:
		if 20 <= x <= 200:
			cmd = 'right ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def forward(x):
	if type(x) is int:
		if 20 <= x <= 200:
			cmd = 'forward ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def back(x):
	if type(x) is int:
		if 20 <= x <= 200:
			cmd = 'back ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が10...200の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def cw(x):
	if type(x) is int:
		if 1 <= x <= 360:
			cmd = 'cw ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が1...360の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def ccw(x):
	if type(x) is int:
		if 1 <= x <= 360:
			cmd = 'ccw ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n....ERROR: ', cmd, ' ....\n')
				sock.close()
		else:
			print('\n...引数の値が1...360の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def set_speed(x):
	if type(x) is int:
		if 10 <= x <= 100:
			cmd = 'speed ' + str(x)
			cmd = cmd.encode(encoding = "utf-8")
			print('Send: ', cmd, ' to ', tello_address)
			try:
				sock.sendto(cmd, tello_address)
				while streaming.main_thread:
					data, server = sock.recvfrom(1518)
					res = data.decode(encoding = "utf-8")
					if res == 'ok' or res == 'error':
						print('recv: ', cmd, ' ', res)
						break
			except socket.error:
				print('\n . . .\n')
				sock.close()
		else:
			print('\n...引数の値が10...100の間でない\n')
	else:
		print('\n...引数の値が整数型でない\n')


def streamon():
	cmd = 'streamon'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				streaming.video_recording = 1
				print('recv: ', cmd, ' ', res)
				break
		time.sleep(5)
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		streaming.video_recording = 0
		sock.close()


def streamoff():
	cmd = 'streamoff'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		# time.sleep(5)
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				streaming.video_recording = 0
				print('recv: ', cmd, ' ', res)
				break
	except socket.error:
		print('\n....ERROR: ', cmd, ' ....\n')
		streaming.video_recording = 0
		sock.close()


def get_qrcode():
	cmd = 'streamon'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: get_qrcode(', cmd, ') to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				streaming.video_recording = 10
				print('recv: get_qrcode(', cmd, ') ', res)
				break
	except socket.error:
		print('\n....ERROR:  QR Code ....\n')
		streaming.video_recording = 0
		sock.close()

	time.sleep(5)
	cmd = 'streamoff'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: get_qrcode(', cmd, ') to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if res == 'ok' or res == 'error':
				streaming.video_recording = 0
				print('recv: get_qrcode(', cmd, ') ', res)
				break
	except socket.error:
		print('\n....ERROR:  QR Code ....\n')
		streaming.video_recording = 0
		sock.close()
	code = streaming.qr_code
	streaming.qr_code = ""
	return code


def get_speed():
	cmd = 'speed?'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if is_float(res):
				res = float(res)
				print('recv: ', cmd, ' ', res, ' cm/s')
				return res
	except socket.error:
		print('\n .......get_speed.........\n')
		sock.close()


def get_battery():
	cmd = 'battery?'
	cmd = cmd.encode(encoding = "utf-8")
	print('Send: ', cmd, ' to ', tello_address)
	try:
		sock.sendto(cmd, tello_address)
		while streaming.main_thread:
			data, server = sock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			if is_int(res):
				res = int(res)
				print('recv: ', cmd, ' ', res, '%')
				return res
	except socket.error:
		print('\n .......get_battery.........\n')
		sock.close()


def end():
	print('Send: end')
	streaming.main_thread = False
	time.sleep(2)
	sock.close()

status.py

#
# Tello Edu (SDK2.0 対応) Python3 status Library
#

import csv
import datetime
import re
import socket
import time
import telloedu.streaming as streaming

def tello_status_thread():
	path = './data/'
	date_fmt = '%Y-%m-%d_%H%M%S'
	file_name = '%stellostatus-%s.csv' % (path, datetime.datetime.now().strftime(date_fmt))
	csvhead = ["mid", "x", "y", "z", "xxx", "pitch", "roll", "yaw", "vgx", "vgy", "vgz", "templ", "temph", "tof",
	           "h", "bat", "baro", "time", "agx", "agy", "sgz"]
	tshost = '0.0.0.0'
	tsport = 8890
	tslocaddr = (tshost, tsport)
	tssock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
	tssock.bind(tslocaddr)
	while streaming.main_thread:
		try:
			data, server = tssock.recvfrom(1518)
			res = data.decode(encoding = "utf-8")
			res2 = re.sub('[a-z:\n\r]', '', res)
			res3 = res2.split(';')
			try:
				with open(file_name, 'x') as f:
					writer = csv.writer(f)
					writer.writerow(csvhead)
					writer.writerow(res3)
			except FileExistsError:
				with open(file_name, 'a') as f:
					writer = csv.writer(f)
					writer.writerow(res3)
			time.sleep(5)
		except socket.error:
			print('\nTello Status Exit . . .\n')
			tssock.close()
			break
		except KeyboardInterrupt:
			print('\nTello Status Exit . . .\n')
			tssock.close()
			break
	tssock.close()

streaming.py

#
# Tello Edu (SDK2.0 対応) Python3 Streaming Library
#

import datetime
import cv2
from PIL import Image
from pyzbar.pyzbar import decode

video_recording = 0
qr_code = ""

# Video Recording Thread
def video_recording_thread():
	global video_recording
	while main_thread:
		try:
			if video_recording == 1:
				video_recording_start()
			if video_recording == 10:
				analyze_qrcode()
			if video_recording == 999:
				break
		except KeyboardInterrupt:
			print('\nExit . . .\n')
			break


def video_recording_start():
	global video_recording
	vr_udp_ip = '0.0.0.0'
	vr_udp_port = 11111
	path = './mpg/'
	date_fmt = '%Y-%m-%d_%H%M%S'
	file_name = '%stello-video-%s.m4v' % (path, datetime.datetime.now().strftime(date_fmt))
	udp_video_address = 'udp://@' + vr_udp_ip + ':' + str(vr_udp_port)

	cap = cv2.VideoCapture(udp_video_address)
	frame_rate = cap.get(cv2.CAP_PROP_FPS)  # 40   フレームレート
	size = (640, 480)  # 動画の画面サイズ
	fmt = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
	writer = cv2.VideoWriter(file_name, fmt, frame_rate, size)

	if not cap.isOpened():
		cap.open(udp_video_address)

	while video_recording == 1:
		try:
			ret, frame = cap.read()
			frame = cv2.resize(frame, size)
			writer.write(frame)
		except cv2.error:
			print('\nExit . . .\n')
			writer.release()
			cap.release()
			break
	writer.release()
	cap.release()


def analyze_qrcode():
	global video_recording
	global qr_code
	qr_code = ""

	if video_recording <= 9:
		return 'analyze qrcode: error'

	vr_udp_ip = '0.0.0.0'
	vr_udp_port = 11111
	path = './img/'
	date_fmt = '%Y-%m-%d_%H%M%S'
	file_name = '%sqrcode-%s.jpg' % (path, datetime.datetime.now().strftime(date_fmt))
	cap = None
	udp_video_address = 'udp://@' + vr_udp_ip + ':' + str(vr_udp_port)

	if cap is None:
		cap = cv2.VideoCapture(udp_video_address)

	if not cap.isOpened():
		cap.open(udp_video_address)

	ret, frame = cap.read()
	cv2.imwrite(file_name, frame)
	qr = decode(Image.open(file_name))
	if len(qr) != 0:
		qr_code = qr[0][0].decode('utf-8', 'ignore')
		print('QR Code: %s' % qr_code)
	else:
		qr_code = ""
		print('No QR Code !!!!!!')
	video_recording = 0
	cap.release()

main_thread = True

telolib.py

#
# Tello Edu (SDK2.0 対応) Python3 Common Library
#

# type check function
def is_int(s):
	try:
		int(s)
		return True
	except ValueError:
		return False


def is_float(s):
	try:
		float(s)
		return True
	except ValueError:
		return False

メイン（プログラム）ファイル

　起動関数が定義されています。このファイルを実行してください。

tellomain.py

#
# Tello Edu (SDK2.0 対応) Python3 Main Library
#

from concurrent import futures
from ugoki import *
from telloedu.command import *
from telloedu.status import *
from telloedu.streaming import *


def start():
	try:
		command()
		gotello()
		end()
	except KeyboardInterrupt:
		emergency()
		print('\n..... Keyboard Interrupt: emergency!! ......\n')
	except TypeError:
		emergency()
		print('\n..... Type Error: emergency!! ......\n')


if __name__ == "__main__":
	with futures.ThreadPoolExecutor(max_workers=4) as executor:
		executor.submit(tello_status_thread)
		executor.submit(video_recording_thread)
		start()

演習用ファイル

　演習では，このgotello関数の内部を課題に応じて変更します。以下のプログラムはサンプルです。ただし，「# ここから以下を直します」と「# これから下は直さないでください」の間以外はさわらないこと。
以下のgotello関数の中は，使えるコマンドなどを一通り試す目的でいれています。

ugoki.py

from telloedu.command import *

def gotello():
	# ここから以下が修正可能です
	takeoff()
	streamon()
	cw(90)
	streamoff()
	land()
	# これから以下は修正できません

gotello関数の内部で使える命令（コマンド）

コマンド	引数	説明	返値
command()	なし	Tello EduのモードをSDK2に設定最初にこのコマンドを送信する必要がある	なし
takeoff()	なし	離陸（約1m上昇する）	なし
land()	なし	着陸	なし
stop()	なし	ホバリング（次のコマンドを50秒以内に送信）	なし
up(x)	x: 20cm〜200cm	上昇	なし
down(x)	x: 20cm〜200cm	下降	なし
left(x)	x: 20cm〜200cm	左側に動く	なし
right(x)	x: 20cm〜200cm	右側に動く	なし
forward(x)	x: 20cm〜200cm	前進	なし
back(x)	x: 20cm〜200cm	後進	なし
cw(x)	x:1度〜360度	時計回りに回転(Clockwise Rotation)	なし
ccw(x)	x:1度〜360度	反時計回りに回転(Counter Clockwise Rotation)	なし
end()	なし	すべてのコマンドを終了（一番最後に送信する必要がある）	なし
streamon()	なし	ビデオ録画開始　※録画中にget_qrcode()を呼び出さないこと	なし
streamoff()	なし	ビデオ録画終了	なし
set_speed(x)	x:10〜100	ドローンの動くスピードを指定	なし
get_qrcode()	なし	QRコードを撮影して解析　※解析中にstreamon()を呼び出さないこと	解析結果（文字列）
get_speed()	なし	ドローンに設定されてるスピード情報を得ることができる	スピード（数値：浮動小数点）
get_battery()	なし	ドローンのバッテリー残量を得ることができる	残量（0〜100）
emergency()	なし	緊急停止（4つのモータを停止させる	なし

実行方法

PuCharmの実行ボタン（左側の緑の三角）で動かせます。緊急停止したい場合は停止ボタン（右側の赤い四角）を押します。

映像を記録などした場合，ファイルへの書き込みなどが終わるなど処理が完了するまで，プログラムが完全に停止しません。すぐに，停止ボタンを押すとファイルへの書き込みが失敗する可能性があります。
完全に終わるまでお待ちください。映像を録画している場合は，約1分ほど待ってください。録画をしていない場合は，10秒ほどまってください。

緊急停止の場合，1回の停止ボタン操作で終わらない場合があります。その場合は，もう一度押してください。

参考資料等

参考にさせて頂いた以下のサイトおよび管理者に感謝いたします。

Tello SDK2.0
Tello3.py（SDK2.0サンプルプログラム）
Telloドローンでプログラミング！ーディープラーニングで物体認識編ー
Pythonの画像処理ライブラリpillowの使い方をわかりやすく解説！
Anacondaを使ってMac OSにOpenCVをインストールする
Pythonでバーコードを読み込む