はじめに
このページは，
の１ページです．
全体を見たい場合は上記ページへお戻りください．
概要
今回は，TelloでARマーカーを検出し，マーカーの番号に応じて離着陸や移動などの動作をさせます．
OpenCVにはARマーカーのライブラリArUcoが標準で入っているので，比較的簡単に実装が可能です．
前提条件
ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます．
Linuxマシンであれば /home/(ユーザー名)/ に，Tello-Pythonというフォルダがあることになります．
詳しくは Tello-Pythonのダウンロード を御覧ください．
ARマーカーって何？
ARマーカーとは，バーコードやQRコードの様な，データを持ったパターン画像の一種です．要は２次元バーコードです．
ARマーカーの使いどころ バーコード・ＱＲコード・ArUco・カメレオンコード
しかしARマーカーは，AR(Augmented Reality，拡張現実感)をデザインするために使うという目的があるので，「AR」の名を冠しています．
ARは，現実を撮影した画像の中に，さながら実物がそこにいるかの様にCGを組み合わせる技術です．スマホゲームでは，ポケモン(ポケモンGO)や恐竜(ジュラシックワールド)が，現実世界にいるかの様に見せる技術ですね．
PCではARToolKit，スマホではARKit，AR Coreといったライブラリを使って作ることが可能です
今回は，OpenCVのライブラリ ArUcoを使ってプログラミングをします．
参考にしたのはこのページです．
contribモジュールのインストール
arucoモジュールはライセンスの都合でcontribとして分離されているので，「Tello-Video」のlinux_install.shでの記述$ sudo pip install opencv-python==3.4.5.20ではインストールされません．
参考：OpenCV3.3(core+contrib)をPythonにインストール
以下のコマンドで，contribも含んだopencvもインストールしてください．
$ sudo pip install opencv-contrib-python==3.4.5.20
これで拡張モジュールを含んだOpenCVの全機能が使えるようになります．
※注意：opencv-pythonと同じバージョンのopencv-contrib-pythonをインストールしましょう．
TelloでARマーカーを認識するプログラム
ディレクトリの作成
まずは，Tello-Pythonディレクトリの下に，新しいディレクトリTello-CV-arを作ります．
$ cd ~/Tello-Python/
$ mkdir Tello-CV-ar
$ cd Tello-CV-ar
ARマーカーの準備
まずは，ARマーカーを作り，印刷しなければなりません．
マーカーファイル出力プログラム
次のプログラムをコピー＆ペーストするか，
ここ を右クリックして[名前を付けて保存]などの機能で保存してください．
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cv2
# ArUcoのライブラリを導入
aruco = cv2.aruco
# 4x4のマーカー，ID番号は50までの辞書を使う
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
def main():
# 10枚のマーカーを作るために10回繰り返す
for i in range(10):
ar_image = aruco.drawMarker(dictionary, i, 150) # ID番号は i ，150x150ピクセルでマーカー画像を作る．
fileName = "ar" + str(i).zfill(2) + ".png" # ファイル名を "ar0x.png" の様に作る
cv2.imwrite(fileName, ar_image) # マーカー画像を保存する
# "python MakeMarker_0to9.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":
main() # メイン関数を実行
このプログラムを実行し，ARマーカーの画像ファイルを作ります．
$ python MakeMarker_0to9.py
$ ls
MakeMarker_0to9.py ar01.png ar03.png ar05.png ar07.png ar09.png main.py
ar00.png ar02.png ar04.png ar06.png ar08.png libh264decoder.so tello.py
プログラムを実行しても，特にメッセージ出力はされません．ですが，lsで見てみると，ar00.png〜ar09.pngまでの画像ファイルができていることがわかります．
一応，ここにも画像を貼っておきます．
ID=0 ID=1 ID=2 ID=3
ID=4 ID=5 ID=6 ID=7
ID=8 ID=9
トレカに貼ると格好良い？
マーカー画像のファイルができたら，印刷しましょう．
印刷オプションで[自動的に拡大縮小]する機能は使わないでください．
滅茶苦茶大きいマーカーがプリントアウトされてきますので．．．(経験者は語る
マーカーは1枚ずつTelloに見せて使うので，カード状になっていると便利です．
筆者は，不要なトレーディングカードを貰ってきて，そこに貼り付けました．
ID番号順に並んでいない理由は，裏返すとこうなっているからです．
IDと機能との対応は，
- ID=0 : 離陸
- ID=1 : 着陸
- ID=2 : 上昇
- ID=3 : 下降
- ID=4 : 左旋回
- ID=5 : 右旋回
- ID=6 : 前進
- ID=7 : 後進
- ID=8 : 左移動
- ID=9 : 右移動
となります．
ついでなので，トレカに貼り付けた裏側の画像も貼っておきます．
パワポで描いて印刷し，ハサミで切ってノリで貼っただけです(^_^
用意した機能は，キーボードでコントロールした時と同様ですね．
ファイルをコピー
tello.pyとlibh264decoder.soを，スケルトンのTello-CV-coreからコピーしてきましょう．
$ cp ../Tello-CV-core/tello.py ./
$ cp ../Tello-CV-core/libh264decoder.so ./
main.pyもTello-CV-coreからコピーしておけば，書き加えるだけなので作業が楽ですね．
$ cp ../Tello-CV-core/main.py ./
main.py
プログラム本体であるmain.pyは，スケルトンプログラムに書き加える形で作成しました．
書き加えの手間を省くなら，以下のコードをコピー＆ペーストするか，
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください．
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello # tello.pyをインポート
import time # time.sleepを使いたいので
import cv2 # OpenCVを使うため
# メイン関数
def main():
# OpenCVが持つARマーカーライブラリ「aruco」を使う
aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) # ARマーカーは「4x4ドット，ID番号50まで」の辞書を使う
# Telloクラスを使って，droneというインスタンス(実体)を作る
drone = tello.Tello('', 8889, command_timeout=.01)
current_time = time.time() # 現在時刻の保存変数
pre_time = current_time # 5秒ごとの'command'送信のための時刻変数
time.sleep(0.5) # 通信が安定するまでちょっと待つ
pre_idno = None # 前回のID番号を記憶する変数
count = 0 # 同じID番号が見えた回数を記憶する変数
#Ctrl+cが押されるまでループ
try:
while True:
# (A)画像取得
frame = drone.read() # 映像を1フレーム取得
if frame is None or frame.size == 0: # 中身がおかしかったら無視
continue
# (B)ここから画像処理
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) # OpenCV用のカラー並びに変換する
small_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更
# ARマーカーの検出と，枠線の描画
corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に，原画像に描画する
# 50回同じマーカーが見えたらコマンド送信する処理
try:
if ids != None: # idsが空(マーカーが１枚も認識されなかった)場合は何もしない
idno = ids[0,0] # idsには複数のマーカーが入っているので，0番目のマーカーを取り出す
if idno == pre_idno: # 今回認識したidnoが前回のpre_idnoと同じ時には処理
count+=1 # 同じマーカーが見えてる限りはカウンタを増やす
if count > 50: # 50回同じマーカーが続いたら，コマンドを確定する
print("ID=%d"%(idno))
if idno == 0:
drone.takeoff() # 離陸
elif idno == 1:
drone.land() # 着陸
time.sleep(3)
elif idno == 2:
drone.move_up(0.3) # 上昇
elif idno == 3:
drone.move_down(0.3) # 下降
elif idno == 4:
drone.rotate_ccw(20) # 左旋回
elif idno == 5:
drone.rotate_cw(20) # 右旋回
elif idno == 6:
drone.move_forward(0.3) # 前進
elif idno == 7:
drone.move_backward(0.3) # 後進
elif idno == 8:
drone.move_left(0.3) # 左移動
elif idno == 9:
drone.move_right(0.3) # 右移動
count = 0 # コマンド送信したらカウント値をリセット
else:
count = 0
pre_idno = idno # 前回のpre_idnoを更新する
else:
count = 0 # 何も見えなくなったらカウント値をリセット
except ValueError, e: # if ids != None の処理で時々エラーが出るので，try exceptで捕まえて無視させる
print("ValueError")
# (X)ウィンドウに表示
cv2.imshow('OpenCV Window', small_image) # ウィンドウに表示するイメージを変えれば色々表示できる
# (Y)OpenCVウィンドウでキー入力を1ms待つ
key = cv2.waitKey(1)
if key == 27: # k が27(ESC)だったらwhileループを脱出，プログラム終了
break
elif key == ord('t'):
drone.takeoff() # 離陸
elif key == ord('l'):
drone.land() # 着陸
elif key == ord('w'):
drone.move_forward(0.3) # 前進
elif key == ord('s'):
drone.move_backward(0.3) # 後進
elif key == ord('a'):
drone.move_left(0.3) # 左移動
elif key == ord('d'):
drone.move_right(0.3) # 右移動
elif key == ord('q'):
drone.rotate_ccw(20) # 左旋回
elif key == ord('e'):
drone.rotate_cw(20) # 右旋回
elif key == ord('r'):
drone.move_up(0.3) # 上昇
elif key == ord('f'):
drone.move_down(0.3) # 下降
# (Z)5秒おきに'command'を送って、死活チェックを通す
current_time = time.time() # 現在時刻を取得
if current_time - pre_time > 5.0 : # 前回時刻から5秒以上経過しているか？
drone.send_command('command') # 'command'送信
pre_time = current_time # 前回時刻を更新
except( KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱
print( "SIGINTを検知" )
# telloクラスを削除
del drone
# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__": # importされると"__main__"は入らないので，実行かimportかを判断できる．
main() # メイン関数を実行
プログラムの実行
プログラム本体はmain.pyです．
$ python main.py
今までと同様にctrl+cを押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう．
実行結果
TelloにARマーカーを１つずつ見せてください．複数同時には対応できていませんので．
また，Telloが動き出したら，すぐにカードを引っ込めてください．「見せっぱなし」にすると思いもよらない動きをします．
また，imshowのOpenCVウィンドウがアクティブならば，スケルトンと同様にキー入力による操縦が可能です．
問題なく動作すれば，以下の様になるはずです．
$ python main.py
sent: command
sent: streamon
[h264 @ 0x1ed7b00] non-existing PPS 0 referenced
[h264 @ 0x1ed7b00] non-existing PPS 0 referenced
[h264 @ 0x1ed7b00] decode_slice_header error
[h264 @ 0x1ed7b00] no frame!
(略)
ID=0 <- ID=0のカードを認識したのでtakeoffコマンドを送信
>> send cmd: takeoff
ID=0
>> send cmd: takeoff
>> send cmd: command <- 5秒ごとのcommand
ID=2
>> send cmd: up 30
>> send cmd: command
ID=2
>> send cmd: up 30
>> send cmd: command
ID=2
>> send cmd: up 30
ID=5
>> send cmd: cw 20
>> send cmd: command
ID=5
>> send cmd: cw 20
>> send cmd: command
カードを認識した際に，コンソールにID番号が表示されます．
実際に動かしている様子の動画は，以下の様になります．
Tello-Pythonをベースにして、ARマーカーを認識させてみた。#tello pic.twitter.com/F1H1pQEYsj
— hsgucci404 (@hsgucci404) October 10, 2019
main.pyの解説
メイン関数以外は，前回のスケルトンプログラムと同じなので，説明は割愛します．
import部分
インポート部分は特に変わりはありません．
import tello # tello.pyをインポート
import time # time.sleepを使いたいので
import cv2 # OpenCVを使うため
ArUcoのライブラリはOpenCVが持っているので，cv2のインポートで済んでしまいます．
メイン関数
メイン関数の中身は大きく分けて３つの部分に分かれています．
「初期化」「ループ」「終了処理」です．
# メイン関数本体
def main():
初期化部
ループ部
終了処理部
それぞれ解説していきます．
初期化部
# OpenCVが持つARマーカーライブラリ「aruco」を使う
aruco = cv2.aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) # ARマーカーは「4x4ドット，ID番号50まで」の辞書を使う
# Telloクラスを使って，droneというインスタンス(実体)を作る
drone = tello.Tello('', 8889, command_timeout=.01)
current_time = time.time() # 現在時刻の保存変数
pre_time = current_time # 5秒ごとの'command'送信のための時刻変数
time.sleep(0.5) # 通信が安定するまでちょっと待つ
pre_idno = None # 前回のID番号を記憶する変数
count = 0 # 同じID番号が見えた回数を記憶する変数
Telloクラス生成や，5秒おきにcommandを送信する機能はいつも通りです．
今回はArUcoの機能を使うので，cv2.arucoでクラスを作っています．
また「4x4ドット，ID=50まで」の辞書を呼び出しています．これは，ARマーカーを作った時と同じ辞書を使わなければ認識できないので注意してください．画像生成側と画像読込側で同じ辞書を使う必要があります．
また，pre_idno(前回のIDナンバー)とcount(回数を数えるカウンタ)という変数を定義しています．
これは，ARマーカーが一瞬見えただけではTelloを動かさないようにするためです．たった1フレームマーカーが見えただけで移動コマンドを送信していたら，見えている間連続してコマンドが送信され先行入力が溜まって暴走する危険性があります．
今回は50回同じマーカーが見え続けたらコマンド送信をするようにしました．そのための変数がpre_idnoとcountです．
ループ部
while Trueで永久ループを作り，ctrl+cを検知をtry exceptでやるのは今までと同様です．
#Ctrl+cが押されるまでループ
try:
while True:
# (A)画像取得
frame = drone.read() # 映像を1フレーム取得
if frame is None or frame.size == 0: # 中身がおかしかったら無視
continue
# (B)ここから画像処理
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) # OpenCV用のカラー並びに変換する
small_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更
# ARマーカーの検出と，枠線の描画
corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に，原画像に描画する
# 50回同じマーカーが見えたらコマンド送信する処理
try:
if ids != None: # idsが空(マーカーが１枚も認識されなかった)場合は何もしない
idno = ids[0,0] # idsには複数のマーカーが入っているので，0番目のマーカーを取り出す
if idno == pre_idno: # 今回認識したidnoが前回のpre_idnoと同じ時には処理
count+=1 # 同じマーカーが見えてる限りはカウンタを増やす
if count > 50: # 50回同じマーカーが続いたら，コマンドを確定する
print("ID=%d"%(idno))
if idno == 0:
drone.takeoff() # 離陸
elif idno == 1:
drone.land() # 着陸
time.sleep(3)
(略)
count = 0 # コマンド送信したらカウント値をリセット
else:
count = 0
pre_idno = idno # 前回のpre_idnoを更新する
else:
count = 0 # 何も見えなくなったらカウント値をリセット
except ValueError, e: # if ids != None の処理で時々エラーが出るので，try exceptで捕まえて無視させる
print("ValueError")
# (X)ウィンドウに表示
cv2.imshow('OpenCV Window', small_image) # ウィンドウに表示するイメージを変えれば色々表示できる
# (Y)OpenCVウィンドウでキー入力を1ms待つ
key = cv2.waitKey(1)
if key == 27: # k が27(ESC)だったらwhileループを脱出，プログラム終了
break
elif key == ord('t'):
drone.takeoff() # 離陸
elif key == ord('l'):
drone.land() # 着陸
(略)
# (Z)5秒おきに'command'を送って、死活チェックを通す
current_time = time.time() # 現在時刻を取得
if current_time - pre_time > 5.0 : # 前回時刻から5秒以上経過しているか？
drone.send_command('command') # 'command'送信
pre_time = current_time # 前回時刻を更新
except( KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱
print( "SIGINTを検知" )
Tello-CV-coreのmain.pyと比べてもらうと分り易いですが，(B)のブロックに色々と書き加えられているだけです．(A),(X),(Y),(Z)はそのままです．
次は(B)ブロックだけ見ていきます．
# (B)ここから画像処理
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) # OpenCV用のカラー並びに変換する
small_image = cv2.resize(image, dsize=(480,360) ) # 画像サイズを半分に変更
# ARマーカーの検出と，枠線の描画
corners, ids, rejectedImgPoints = aruco.detectMarkers(small_image, dictionary) #マーカを検出
aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0)) #検出したマーカ情報を元に，原画像に描画する
# 50回同じマーカーが見えたらコマンド送信する処理
try:
if ids != None: # idsが空(マーカーが１枚も認識されなかった)場合は何もしない
idno = ids[0,0] # idsには複数のマーカーが入っているので，0番目のマーカーを取り出す
if idno == pre_idno: # 今回認識したidnoが前回のpre_idnoと同じ時には処理
count+=1 # 同じマーカーが見えてる限りはカウンタを増やす
if count > 50: # 50回同じマーカーが続いたら，コマンドを確定する
print("ID=%d"%(idno))
if idno == 0:
drone.takeoff() # 離陸
elif idno == 1:
drone.land() # 着陸
time.sleep(3)
elif idno == 2:
drone.move_up(0.3) # 上昇
elif idno == 3:
drone.move_down(0.3) # 下降
elif idno == 4:
drone.rotate_ccw(20) # 左旋回
elif idno == 5:
drone.rotate_cw(20) # 右旋回
elif idno == 6:
drone.move_forward(0.3) # 前進
elif idno == 7:
drone.move_backward(0.3) # 後進
elif idno == 8:
drone.move_left(0.3) # 左移動
elif idno == 9:
drone.move_right(0.3) # 右移動
count = 0 # コマンド送信したらカウント値をリセット
else:
count = 0
pre_idno = idno # 前回のpre_idnoを更新する
else:
count = 0 # 何も見えなくなったらカウント値をリセット
except ValueError, e: # if ids != None の処理で時々エラーが出るので，try exceptで捕まえて無視させる
print("ValueError")
画像をBGR並びにして縮小し，small_imageという画像を作るまでは，Tello-CV-coreと同じです．
ARマーカーの認識をaruco.detectMarkers(small_image, dictionary)で行っています．small_image画像を，初期化部で用意したAR辞書dictionaryで処理しています．
detectMarkersの処理結果を使って，aruco.drawDetectedMarkers(small_image, corners, ids, (0,255,0))で画面上に枠の描画を行っています．今回は(B,G,R)=(0,255,0)の色です．
検出できた全てのマーカーのIDがidsに配列として入っているので，1番先頭に入っているマーカーを取り出すことにしました．
このプログラムをもっと拡張するのであれば，複数枚見えた時は「一番大きく見えているマーカーを使う」，という処理にすれば，より汎用性が向上すると思います．その場合，cornersの中身から一番大きな枠を作るインデックスを探す処理を追加する必要があります．
１フレーム前に検出したID番号と，今回検出したID番号と番号を比較して，同じ時だけ作業を行います．
カードが代わったらリセットです．
同じカードを50回検出することができたら，Telloへのコマンド送信を行っています．
フローで書くと，こんな感じです．
この50回という回数を変化させることで，反応を早くしたり遅くしたりできます．
「何回連続して検出したかをカウントする」というのは，よくあるプログラミングのテクニックです．「キーボードやジョイパッドボタンの長押し」の検出でもよく使われます．もちろん，検出処理をもっとスマートに書くこともできますが，今回は入門なので「シンプル・読み易く」を追求しました．
新しいコマンドを増やしたい場合，ここのelif文でID番号を追加します．「(前後左右への)宙返り」のコマンドを追加できそうですね．
おわりに
今回はArUcoライブラリを使ってARマーカーを認識し，Telloに見せるとTelloが動く，というプログラムを作りました．
これ，子供にやらせると結構喜んでくれますよ(^^
今回はARマーカー本来の使い方「映像にCGを重ねる」とは違いますが，ARマーカー認識の基本となります．
カメラキャリブレーションや，マーカーの角度検出などへと発展すれば，部屋や障害物にマーカーを貼ることでロボットの自己位置を予想する事も可能になります．
参考ページを貼っておきます．
・pythonでカメラ行列と歪みパラメータの取得
・pythonでARマーカーの姿勢推定
「ID番号xxのマーカーが角度r,p,yで見えているという事は，自分は今x,yの位置にいることになる．」
「ID番号xxのマーカーは荷物だ．これを持ち上げる必要がある．」
なんて使い方ができるわけです．