やったこと
TelloドローンをMacbookとWifiでつなぎ、Macbookからのキーボード操作で、Telloを遠隔操作。
同時に、Tello搭載の単眼カメラから受信したフレーム画像をMacbook側でリアルタイムに画像処理し、結果をウィンドウに描画。
ウィンドウは左右2領域に分割した1つの画面で、左側が画像キャプション文を埋め込んだ画像で、右側が物体検出(矩形表示)と人物検出人数・Tello飛行高度を文字列表記した画像。
Telloドローン飛行撮影中のTerminal実行画面(一部)
electron@diynoMacBook-Pro examples % python3 tello_drone_caption_image.py
[INFO] tello.py - 107 - Tello instance was initialized. Host: '192.168.10.1'. Port: '8889'.
[INFO] tello.py - 422 - Send command: 'command'
[INFO] tello.py - 446 - Response command: 'ok'
[INFO] tello.py - 422 - Send command: 'streamon'
[INFO] tello.py - 446 - Response streamon: 'ok'
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] non-existing PPS 0 referenced
[h264 @ 0x7fba6b299c00] decode_slice_header error
[h264 @ 0x7fba6b299c00] no frame!
[h264 @ 0x7fba6b29b000] error while decoding MB 43 40, bytestream -8
Num of detected person(s) is 0
ToF Distane 10 cm
Height 0 cm
0.1秒以内に操作コマンドを入力して下さい :[h264 @ 0x7fba6b2b3a00] left block unavailable for requested intra mode
[h264 @ 0x7fba6b2b3a00] error while decoding MB 0 36, bytestream 340
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 10 cm
Height 0 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
i
Num of detected person(s) is 0
ToF Distane 10 cm
Height 0 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: i を受信しました。
[INFO] tello.py - 422 - Send command: 'takeoff'
[h264 @ 0x7fba6b2b2e00] error while decoding MB 7 37, bytestream -8
[INFO] tello.py - 446 - Response takeoff: 'ok'
r
r
Num of detected person(s) is 0
ToF Distane 79 cm
Height 50 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: r を受信しました。
[INFO] tello.py - 422 - Send command: 'up 30'
[INFO] tello.py - 446 - Response up 30: 'ok'
Num of detected person(s) is 1
ToF Distane 104 cm
Height 80 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: r を受信しました。
[INFO] tello.py - 422 - Send command: 'up 30'
d
[INFO] tello.py - 446 - Response up 30: 'ok'
Num of detected person(s) is 0
ToF Distane 128 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: d を受信しました。
[INFO] tello.py - 422 - Send command: 'right 30'
d
[INFO] tello.py - 446 - Response right 30: 'ok'
Num of detected person(s) is 1
ToF Distane 131 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: d を受信しました。
[INFO] tello.py - 422 - Send command: 'right 30'
[INFO] tello.py - 446 - Response right 30: 'ok'
Num of detected person(s) is 1
ToF Distane 134 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 1
ToF Distane 132 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 128 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
a
Num of detected person(s) is 0
ToF Distane 134 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: a を受信しました。
[INFO] tello.py - 422 - Send command: 'left 30'
[INFO] tello.py - 446 - Response left 30: 'ok'
Num of detected person(s) is 4
ToF Distane 130 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 3
ToF Distane 132 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
a
Num of detected person(s) is 4
ToF Distane 131 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド: a を受信しました。
[INFO] tello.py - 422 - Send command: 'left 30'
[INFO] tello.py - 446 - Response left 30: 'ok'
Num of detected person(s) is 0
ToF Distane 133 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 129 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 135 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 132 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 132 cm
Height 100 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 98 cm
Height 70 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 1
ToF Distane 10 cm
Height 0 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
Num of detected person(s) is 0
ToF Distane 10 cm
Height 0 cm
0.1秒以内に操作コマンドを入力して下さい :
操作コマンド入力時間切れ。次のフレーム画像を読み込みます。
( 以下略 )
( 考察 )
Telloの動きから、2〜3秒ほど、ウインドウへの画面描画が遅延しました。
説明文の生成と描画は、1秒経たないうちにテキパキと切り替わり、処理速度には問題ない模様。
なお、前の記事では、Telloからの受信画像をOpenCV2の組込関数で色反転したり、エッジ検出した画像と、別のスクリプトファイルに記述した単眼深度推定を行った。そのときは、複数領域画面に描画した際は、これらの描画は、Telloの動作から遅延は感じられなかった。
今回、Telloの動作と、画面の描画との間に、体感時間でおよそ2〜3秒ほどの遅延が生じたことは、画像説明文の生成処理に、2〜3秒程度の時間を常時、要しているのかもしれない。Telloからの受信画像を扱わずに、ローカルのフォルダに置かれた画像ファイルを読み込んで、画像説明文を生成する場合も、Terminalに説明文が表示されるまで3〜5秒ほどの時間がかかる(ファイル読み込みにかかる時間も含まれているはずだ)。
数秒の遅延は伴うものの、「説明文の生成と描画は、1秒経たないうちにテキパキと切り替わ」ることから、数秒遅れてTelloの一人称視点画像が届けば問題ない運用環境であれば、使い物にはなると思われる。
今回動かしたスクリプト・ファイル
( ディレクトリ構成 )
electron@diynoMacBook-Pro examples % pwd
/Users/electron/Desktop/DJITelloPy_copy/examples
electron@diynoMacBook-Pro examples %
electron@diynoMacBook-Pro examples % tree
.
├── __pycache__
│ ├── build_vocab.cpython-39.pyc
│ ├── create_caption_embedded_image.cpython-39.pyc
│ └── model.cpython-39.pyc
├── build_vocab.py
├── create_caption_embedded_image.py
├── data
│ └── vocab.pkl
├── keyboard-control-movie.py
├── keyboard-control-multi_window.py
├── keyboard-control-multi_window_input_text.py
├── keyboard-control-multi_window_input_text_2.py
├── manual-control-opencv.py
├── manual-control-opencv_2.py
├── manual-control-opencv_3.py
├── manual-control-pygame.py
├── mission-pads.py
├── model.py
├── models
│ ├── decoder-2-1000.ckpt
│ ├── decoder-5-3000.pkl
│ ├── encoder-2-1000.ckpt
│ └── encoder-5-3000.pkl
├── record-video.py
├── simple-swarm.py
├── simple.py
├── take-picture.py
└── tello_drone_caption_image.py
electron@diynoMacBook-Pro examples %
スクリプト・ファイル ①
from timeout_decorator import timeout, TimeoutError
from djitellopy import Tello
import cv2, math, time
import matplotlib.pyplot as plt
import matplotlib.pyplot as plt
import cvlib as cvl
import datetime
from cvlib.object_detection import draw_bbox
# https://niwakomablog.com/python-importerror-handling/
# https://genchan.net/it/programming/python/4949/
import sys
from create_caption_embedded_image import *
# 以下はうまくいかない(2階層上(「...」)にあるcatrディレクトリ配下のpredict_output_read_from_djitello_controllerを指定している)
# Python2系ではできたが、3系ではrelative importはできなくなった。
# https://qiita.com/ysk24ok/items/2711295d83218c699276
# from ...catr import predict_output_read_from_djitello_controller
# ImportError: attempted relative import with no known parent package
# from ...catr import predict_output_read_from_djitello_controller
TIMEOUT_SEC = 0.1
@timeout(TIMEOUT_SEC)
def input_with_timeout(msg=None):
return input(msg)
tello = Tello()
tello.connect()
tello.streamon()
frame_read = tello.get_frame_read()
# tello.takeoff()
while True:
# In reality you want to display frames in a seperate thread. Otherwise
# they will freeze while the drone moves.
img = frame_read.frame
#cv2.imshow("drone", img)
#cv2.imshow('Canny', cv2.Canny(img, 100, 200))
#bitwised_img = cv2.bitwise_not(img)
#cv2.imshow('Bitwised', bitwised_img)
image = img.copy()
#キャプション文が埋込まれた画像を取得
img_caption_embedded = create_caption_embedded_image(img)
#cv2.imshow("caption", img_caption_embedded)
# 物体検出矩形表示と人物検出人数の文字列埋込み表示の画像を取得
label_name = "person"
bbox, label, conf = cvl.detect_common_objects(image)
output_image = draw_bbox(image, bbox, label, conf)
#plt.imshow(output_image)
#plt.show()
#dt_now = datetime.datetime.now()
message = "Num of detected {0}(s) is {1}".format(label_name, str(label.count(label_name)))
input_text_0 = message
#cv2.putText(output_image, str(input_text_0), (0, 50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
#https://djitellopy.readthedocs.io/en/latest/tello/#djitellopy.tello.Tello.query_battery
time_of_flight_distance_senser_val = tello.get_distance_tof()
input_text_1 = "ToF Distane {0} cm".format(time_of_flight_distance_senser_val)
height= tello.get_height()
input_text_2 = "Height {0} cm".format(height)
# Terminal標準出力
print(input_text_0)
print(input_text_1)
print(input_text_2)
# カメラ画像にTelloの現在高度(ToFセンサ計測距離(cm)、高さ(cm))を埋込む
cv2.putText(output_image, str(input_text_1), (0, 100), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(output_image, str(input_text_2), (0, 150), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
#cv2.imshow("Video", output_image)
merged_image_group = cv2.hconcat((img_caption_embedded, output_image))
cv2.imshow("Window", merged_image_group)
#plt.imshow(img)
#plt.show()
#output_frame_image_file_name = "dummy"
#cv2.imwrite(output_frame_image_file_name, img)
#次の行(key = cv2.・・・)を削除すると、画像が受信できなくなる。
key = cv2.waitKey(1) & 0xff
try:
msg = input_with_timeout('\n{}秒以内に操作コマンドを入力して下さい :'.format(TIMEOUT_SEC))
print('\n操作コマンド: {} を受信しました。\n'.format(msg))
if msg == "i":
tello.takeoff()
elif msg == "w":
tello.move_forward(30)
elif msg == "s":
tello.move_back(30)
elif msg == "a":
tello.move_left(30)
elif msg == "d":
tello.move_right(30)
elif msg == "e":
tello.rotate_clockwise(30)
elif msg == "q":
tello.rotate_counter_clockwise(30)
elif msg == "r":
tello.move_up(30)
elif msg == "f":
tello.move_down(30)
elif msg == "g":
tello.land()
except TimeoutError:
print('\n操作コマンド入力時間切れ。次のフレーム画像を読み込みます。\n')
tello.land()
スクリプト・ファイル ②
import torch
# import matplotlib.pyplot as plt
import numpy as np
import argparse
import pickle
import os
import cv2
import textwrap
import datetime
import locale
import numpy as np
from torchvision import transforms
from build_vocab import Vocabulary
from model import EncoderCNN, DecoderRNN
from PIL import Image
# Device configuration
device = torch.device('cuda' if torch.cuda.is_available() else 'cpu')
def create_caption_embedded_image(image_data):
# Image preprocessing
transform = transforms.Compose([
transforms.ToTensor(),
transforms.Normalize((0.485, 0.456, 0.406),
(0.229, 0.224, 0.225))])
# Load vocabulary wrapper
vocab_path = 'data/vocab.pkl'
encoder_path = 'models/encoder-5-3000.pkl'
decoder_path = 'models/decoder-5-3000.pkl'
embed_size = 256
hidden_size = 512
num_layers = 1
with open(vocab_path, 'rb') as f:
vocab = pickle.load(f)
# Build models
encoder = EncoderCNN(embed_size).eval() # eval mode (batchnorm uses moving mean/variance)
decoder = DecoderRNN(embed_size, hidden_size, len(vocab), num_layers)
encoder = encoder.to(device)
decoder = decoder.to(device)
# Load the trained model parameters
encoder.load_state_dict(torch.load(encoder_path))
decoder.load_state_dict(torch.load(decoder_path))
# Prepare an image
#image = Image.open(image_path).convert('RGB')
image = Image.fromarray(image_data)
image = image.resize([224, 224], Image.LANCZOS)
image = transform(image).unsqueeze(0)
#image = load_image(args.image, transform)
image_tensor = image.to(device)
# Generate an caption from the image
feature = encoder(image_tensor)
sampled_ids = decoder.sample(feature)
sampled_ids = sampled_ids[0].cpu().numpy() # (1, max_seq_length) -> (max_seq_length)
# Convert word_ids to words
sampled_caption = []
for word_id in sampled_ids:
word = vocab.idx2word[word_id]
sampled_caption.append(word)
if word == '<end>':
break
sentence = ' '.join(sampled_caption)
sentence = sentence.replace("<start>", "")
sentence = sentence.replace("<end>", "")
# Print out the image and the generated caption
#print(sentence)
caption_text_wrap_list = textwrap.wrap(str(sentence), 40)
# 入力した画像ファイルに、生成したキャプション文の文字列を埋込む
if len(caption_text_wrap_list) == 1:
cv2.putText(image_data, str(caption_text_wrap_list[0]), (0, 50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
elif len(caption_text_wrap_list) ==2 :
cv2.putText(image_data, str(caption_text_wrap_list[0]), (0, 50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(image_data, str(caption_text_wrap_list[1]), (0, 90), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
elif len(caption_text_wrap_list) >= 3:
cv2.putText(image_data, str(caption_text_wrap_list[0]), (0, 50),
cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(image_data, str(caption_text_wrap_list[1]), (0, 90), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
cv2.putText(image_data, str(caption_text_wrap_list[2]), (0, 130), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
return image_data
# args.models
# parser.add_argument('--encoder_path', type=str, default='models/encoder-5-3000.pkl', help='path for trained encoder')
# parser.add_argument('--decoder_path', type=str, default='models/decoder-5-3000.pkl', help='path for trained decoder')
# parser.add_argument('--vocab_path', type=str, default='data/vocab.pkl', help='path for vocabulary wrapper')
# Model parameters (should be same as paramters in train.py)
# parser.add_argument('--embed_size', type=int , default=256, help='dimension of word embedding vectors')
# parser.add_argument('--hidden_size', type=int , default=512, help='dimension of lstm hidden states')
# parser.add_argument('--num_layers', type=int , default=1, help='number of layers in lstm')
( 後日追記 )
表示されるウィンドウのサイズが小さいので、大きくしました。
ウィンドウの設定まわりを以下に変更
height = merged_image_group.shape[0] width = merged_image_group.shape[1] resized_output_img = cv2.resize(merged_image_group, (int(2*width), int(2*height))) cv2.namedWindow("Video", cv2.WINDOW_NORMAL) cv2.imshow("Video", resized_output_img)
( 修正後のコード全文 )
- ファイル名を、以下に変更
( 変更前 )
tello_drone_caption_image.py
( 変更後 )
tello_drone_caption_image_large_window.py
from timeout_decorator import timeout, TimeoutError
from djitellopy import Tello
import cv2, math, time
import matplotlib.pyplot as plt
import matplotlib.pyplot as plt
import cvlib as cvl
import datetime
from cvlib.object_detection import draw_bbox
# https://niwakomablog.com/python-importerror-handling/
# https://genchan.net/it/programming/python/4949/
import sys
from create_caption_embedded_image import *
# 以下はうまくいかない(2階層上(「...」)にあるcatrディレクトリ配下のpredict_output_read_from_djitello_controllerを指定している)
# Python2系ではできたが、3系ではrelative importはできなくなった。
# https://qiita.com/ysk24ok/items/2711295d83218c699276
# from ...catr import predict_output_read_from_djitello_controller
# ImportError: attempted relative import with no known parent package
# from ...catr import predict_output_read_from_djitello_controller
TIMEOUT_SEC = 0.1
@timeout(TIMEOUT_SEC)
def input_with_timeout(msg=None):
return input(msg)
tello = Tello()
tello.connect()
tello.streamon()
frame_read = tello.get_frame_read()
# tello.takeoff()
while True:
# In reality you want to display frames in a seperate thread. Otherwise
# they will freeze while the drone moves.
img = frame_read.frame
#cv2.imshow("drone", img)
#cv2.imshow('Canny', cv2.Canny(img, 100, 200))
#bitwised_img = cv2.bitwise_not(img)
#cv2.imshow('Bitwised', bitwised_img)
image = img.copy()
#キャプション文が埋込まれた画像を取得
img_caption_embedded = create_caption_embedded_image(img)
#cv2.imshow("caption", img_caption_embedded)
# 物体検出矩形表示と人物検出人数の文字列埋込み表示の画像を取得
label_name = "person"
bbox, label, conf = cvl.detect_common_objects(image)
output_image = draw_bbox(image, bbox, label, conf)
#plt.imshow(output_image)
#plt.show()
#dt_now = datetime.datetime.now()
message = "Num of detected {0}(s) is {1}".format(label_name, str(label.count(label_name)))
input_text_0 = message
#cv2.putText(output_image, str(input_text_0), (0, 50), cv2.FONT_HERSHEY_TRIPLEX, 1, (0, 0, 255), 1, cv2.LINE_AA)
#https://djitellopy.readthedocs.io/en/latest/tello/#djitellopy.tello.Tello.query_battery
time_of_flight_distance_senser_val = tello.get_distance_tof()
input_text_1 = "ToF Distane {0} cm".format(time_of_flight_distance_senser_val)
height= tello.get_height()
input_text_2 = "Height {0} cm".format(height)
# Terminal標準出力
print(input_text_0)
print(input_text_1)
print(input_text_2)
# カメラ画像にTelloの現在高度(ToFセンサ計測距離(cm)、高さ(cm))を埋込む
cv2.putText(output_image, str(input_text_1), (0, 100), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(output_image, str(input_text_2), (0, 150), cv2.FONT_HERSHEY_TRIPLEX, 1, (255, 255, 255), 1, cv2.LINE_AA)
#cv2.imshow("Video", output_image)
merged_image_group = cv2.hconcat((img_caption_embedded, output_image))
height = merged_image_group.shape[0]
width = merged_image_group.shape[1]
resized_output_img = cv2.resize(merged_image_group, (int(2*width), int(2*height)))
cv2.namedWindow("Video", cv2.WINDOW_NORMAL)
cv2.imshow("Video", resized_output_img)
#次の行(key = cv2.・・・)を削除すると、画像が受信できなくなる。
key = cv2.waitKey(1) & 0xff
try:
msg = input_with_timeout('\n{}秒以内に操作コマンドを入力して下さい :'.format(TIMEOUT_SEC))
print('\n操作コマンド: {} を受信しました。\n'.format(msg))
if msg == "i":
tello.takeoff()
elif msg == "w":
tello.move_forward(30)
elif msg == "s":
tello.move_back(30)
elif msg == "a":
tello.move_left(30)
elif msg == "d":
tello.move_right(30)
elif msg == "e":
tello.rotate_clockwise(30)
elif msg == "q":
tello.rotate_counter_clockwise(30)
elif msg == "r":
tello.move_up(30)
elif msg == "f":
tello.move_down(30)
elif msg == "g":
tello.land()
except TimeoutError:
print('\n操作コマンド入力時間切れ。次のフレーム画像を読み込みます。\n')
tello.land()