OpenCVを使うスケルトンプログラム

#はじめに
このページは，

[公式SDK「Tello-Python」を試そう](https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c)

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要

これまでの記事では，tello.pyのTelloクラスを使って，Telloをキーボードで操作したり，ジョイスティックで操作したりしました．これはTelloを動かす「出力」側の基礎です．

Telloを「自律移動ロボット」にさせるためには，カメラ映像を画像処理してTelloの動きにフィードバックさせる必要があります．すなわち「入力」や「制御」が必要になります．

今回はこの「入力」部分のファーストステップとして，<font color="blue">**スケルトンプログラム**</font>を作ります．
**スケルトンプログラム**とは，言葉通り「スケルトン＝骨格」のプログラムを意味しています．骨格が体になるためには肉をつける必要があるのと同様に，スケルトンプログラムは最低限動作するプログラムではあるものの，「特別な機能＝肉」を持ちません．骨格をベースにして肉を付けていく事で，本来望むプログラムへと発展します．

簡単に言えば<font color="red">**最低動作プログラム**</font>になります．厳密に言えば「**Telloクラスを使ってOpenCVで処理するための準備が完了した最低動作プログラム**」です．ヒストグラム平均化・２値化・ラベリング・顔検出・ARマーカーなどの処理は，スケルトンをベースに作ることになります．

#前提条件

ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます．

Linuxマシンであれば　`/home/(ユーザー名)/`　に，`Tello-Python`というフォルダがあることになります．

詳しくは　[Tello-Pythonのダウンロード](https://qiita.com/hsgucci/items/3327cc29ddf10a321f3c#tello-python%E3%81%AE%E3%83%80%E3%82%A6%E3%83%B3%E3%83%AD%E3%83%BC%E3%83%89)　を御覧ください．

#TelloでOpenCV(画像処理)をするための最低動作プログラム

##ディレクトリの作成

まずは，`Tello-Python`ディレクトリの下に，新しいディレクトリ`Tello-CV-core`を作ります．

```python:Tello-CV-coreディレクトリを作成
$ cd ~/Tello-Python/
$ mkdir Tello-CV-core
$ cd Tello-CV-core
```

##ファイルをコピー

tello.pyとlibh264decoder.soを，`Tello-battery`からコピーしてきましょう．

```bash:重要なファイルをコピー
$ cp ../Tello-battery/tello.py ./
$ cp ../Tello-battery/libh264decoder.so ./
```


##main.py


それでは，プログラム本体である`main.py`です．

以下のコードをコピー＆ペーストするか，
[ここ](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_CV_core/main.py)　を右クリックして[名前を付けて保存]機能でファイル保存してください．


```python:main.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import cv2			# OpenCVを使うため

# メイン関数
def main():
	# Telloクラスを使って，droneというインスタンス(実体)を作る
	drone = tello.Tello('', 8889, command_timeout=.01)  

	current_time = time.time()	# 現在時刻の保存変数
	pre_time = current_time		# 5秒ごとの'command'送信のための時刻変数

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ
	
	#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) )	# 画像サイズを半分に変更


			# (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:プログラムの実行
$ python main.py
```
今までと同様に`ctrl+c`を押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウで`ESC`キーを押して終了するのが良いでしょう．


##実行結果

問題なく動作すれば，以下の様になるはずです．

```bash:実行結果
$ python main.py
sent: command
sent: streamon
[h264 @ 0x1e571e0] non-existing PPS 0 referenced
[h264 @ 0x1e571e0] non-existing PPS 0 referenced
[h264 @ 0x1e571e0] decode_slice_header error
[h264 @ 0x1e571e0] no frame!
[h264 @ 0x1e571e0] non-existing PPS 0 referenced
[h264 @ 0x1e571e0] non-existing PPS 0 referenced
[h264 @ 0x1e571e0] decode_slice_header error
[h264 @ 0x1e571e0] no frame!
(略)
>> send cmd: command
[h264 @ 0x1e571e0] concealing 267 DC, 267 AC, 267 MV errors in P frame
>> send cmd: command
[h264 @ 0x1e571e0] concealing 742 DC, 742 AC, 742 MV errors in P frame
>> send cmd: command
>> send cmd: command
>> send cmd: command
.
.
.
```
コンソール画面は，コマンドの送信結果が自動的に表示されるだけで，特に変化は起こりません．

基本操作は，OpenCVによって作られた画像ウィンドウを使います．

![cv_core.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/da5e1fc6-78dd-6e23-7174-481c9bf0b575.png)

上図のウィンドウをクリックしてアクティブにしてあれば，

- `ESC`キーでプログラム終了
- `t`キーで離陸，`l`キーで着陸
- `w`キーで前進，`s`キーで後進
- `a`キーで左移動，`d`キーで右移動
- `q`キーで左旋回，`e`キーで右旋回
- `r`キーで上昇，`r`キーで下降

ができます．
![key_control.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/e98636e9-42c1-3ec5-34d5-e50b08935b39.png)
これは，キー入力で操縦した記事と同じですね．

#main.pyの解説

ではmain.pyの中身を見てみます．

##import部分

まずはインポート部分です。

```bash:インポート
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import cv2			# OpenCVを使うため
```

OpenCVで画像処理をするために，cv2を読み込んでいます。


##メイン関数

メイン関数の中身は大きく分けて３つの部分に分かれています．
「初期化」「ループ」「終了処理」です．

```python:メイン関数
# メイン関数本体
def main():
    初期化部

    ループ部

    終了処理部
```
それぞれ解説していきます．

###初期化部

```python:初期化処理部
	# Telloクラスを使って，droneというインスタンス(実体)を作る
	drone = tello.Tello('', 8889, command_timeout=.01)  

	current_time = time.time()	# 現在時刻の保存変数
	pre_time = current_time		# 5秒ごとの'command'送信のための時刻変数

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ
```
ジョイスティックの際と同様に，Telloクラスの呼び出し時の引数に`command_timeout=.01`を追加しました．今後はこの引数をいつも付けるようにしようと思います．

また，キーボード入力で操縦の際に行っていた，5秒おきに`command`を送信する機能を付けました．今回はOpenCV画面を出すだけのプログラムなので，Telloの15秒ルールを回避する必要があります．

###ループ部

`while True`で永久ループを作り，ctrl+cを検知を`try except`でやるのは今までと同様です．

```python:ループ部
	#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) )	# 画像サイズを半分に変更


			# (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)		# 前進
			(略)

			# (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を検知" )

```
コメントで(A),(B),(X),(Y),(Z)の５つの作業ブロックに分けています．
それぞれの役割は，

(A):画像の取得.`frame`に画像が入っている
(B):画像処理の本体．ここを書き換えて，結果を(X)で表示するように書くだけ．
(X):`cv2.imshow`で画像をウィンドウに表示
(Y):`cv2.waitKey`でキーボード入力を取って，様々な動作を行う
(Z):5秒毎の`command`送信

今後，様々な画像処理を行う際には，基本的には(B)のブロックだけ書き変えます．
それ以外のブロックは，基本的には触る必要はありません．(応用的には，(Y)で使うキーの種類を追加することがある．)

今回の(B)では，

- `cv2.cvtColor`によるRGBカラープレーンの入れ替え
Telloクラスから渡されるframeは，`cv2.imshow`で表示されると変な色になります．なので`cvtColor`で正しい色の順番に入れ替える必要があります．

- `cv2.resize`による画像の縮小
frame画像は960x720のHD画質なので，半分の480x360に縮小しています．CPUパワーとメモリが潤沢なＰＣでない限りは，HD画質のまま処理するのは厳しいので，画像を小さくしておいたほうが良いでしょう．(一昔前の画像処理は320x240とかで行われていました)

を行っています．
この２つの処理は，今後も必ず行っておいたほうが良いでしょう．

###終了処理部
終了処理はクラスを削除だけです．

```python:終了処理部
	del drone	# telloクラスを削除
```

#おわりに

今回は,OpenCVを使って画像処理をする最低動作プログラム「スケルトンプログラム」を作成しました．

「Tello_Video」の様にわざわざTkinterでGUI画面を作らなくても，`cv2.imshow`で画面表示をさせ，`cv2.waitKey`でキー入力すれば十分であることがわかったと思います．
何よりこっちの方がプログラムが読みやすいし．イベントドリブン型のプログラムは筆者のような古い人間には厳しいので(T_T)

スケルトンプログラムが出来てしまえば，後はOpenCVで自由に画像処理をするだけです．OpenCVに精通した方であれば，造作もないことでしょう．
今後は，参考例として色々なOpenCV関数を使った実例を紹介していきます．