キー入力操縦プログラム

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

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

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

#概要

前回の記事では，tello.pyのTelloクラスを使って，Tello本体のバッテリー残量を取得して表示しました．
．．．でも情報を見るだけなんて，つまらないですね．
やはりドローンを飛ばさなければ面白くありません．

今回は，キーボード入力でTelloを離着陸，移動させてみます．

#前提条件

ホームフォルダに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をキーボードで操縦するプログラム

##ディレクトリの作成

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

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

##ファイルをコピー

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

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

##キーボード入力を取る方法

kbhit.pyは，コンソールでキー入力が欲しい時に便利です．

tello_state.pyでも使っているcursesライブラリでもキー入力を取ることはできますが，コンソール画面にもう１枚別の黒スクリーンが貼られている感じが嫌いな人もいるかと思います．
C言語からの人間は`kbhit`と`getch`で十分なんですよ（笑

kbhit.pyについては，　[dronekitの記事](https://qiita.com/hsgucci/items/e72ef6c172b9d2e62d72#%E3%82%AD%E3%83%BC%E3%83%9C%E3%83%BC%E3%83%89%E5%85%A5%E5%8A%9B%E3%82%92%E5%8F%96%E3%82%8B%E6%96%B9%E6%B3%95)　でも紹介しています．

まずは次のリンクを右クリックし，[名前を付けて保存]の機能を使って，`Tello_key`ディレクトリにkbhit.pyを保存しましょう．

　　　[kbhit.py](https://raw.githubusercontent.com/hsgucci404/tello/master/Tello_key/kbhit.py)

kbhit.pyの使い方は，以下の3つを満たすように書くことです．

(1) kbhit.pyをインポートする

```py
from kbhit import *     
```
(2) プログラムの冒頭(インポートの後)に，この２行を書く

```py
atexit.register(set_normal_term)
set_curses_term()
```
(3) 永久ループ内でキー入力があるかどうかチェック（Ｃ言語と同じ書き方）

```py
if kbhit():     # 何かキーが押されるのを待つ
    key = getch()   # 1文字取得
```

まさに`kbhit`と`getch`！，古い人間にはこれで十分です（爆

##Tello操作のキー配置

「Tello_Video」と「Tello_Video_With_Pose_Recognition」の操作パネルでは，矢印キーも使って操作するキー配置になっていました．モード２の操作に似せてある感じでした．
しかし，**せっかくパソコンで操作するのですから，FPSの操作系で良いじゃないですか！**　やっぱり右手はマウスの上じゃないと．

というわけで，今回作成するプログラムは，下図の様な操作系にしようと思います．
![key_control.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/256470/e98636e9-42c1-3ec5-34d5-e50b08935b39.png)

Takeoffの`t`とLandの`l`だけ，頭文字のキーにしています．

##main.py

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

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

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

# メイン関数本体
def main():
	# kbhitのためのおまじない
	atexit.register(set_normal_term)
	set_curses_term()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	drone = tello.Tello('', 8889) 

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

	#Ctrl+cが押されるまでループ
	try:
		while True:
			if kbhit():     # 何かキーが押されるのを待つ
				key = getch()   # 1文字取得

				# キーに応じた処理
				if key == 't':		# 離陸
					drone.takeoff()
				elif key == 'l':	# 着陸
					drone.land()
				elif key == 'w':	# 前進
					drone.move_forward(0.3)	# 0.3mなので30cm動く
				elif key == 's':	# 後進
					drone.move_backward(0.3)
				elif key == 'a':	# 左移動
					drone.move_left(0.3)
				elif key == 'd':	# 右移動
					drone.move_right(0.3)
				elif key == 'q':	# 左旋回
					drone.rotate_ccw(20)	# 20度旋回
				elif key == 'e':	# 右旋回
					drone.rotate_cw(20)
				elif key == 'r':	# 上昇
					drone.move_up(0.3)
				elif key == 'f':	# 下降
					drone.move_down(0.3)

			time.sleep(0.3)	# 適度にウェイトを入れてCPU負荷を下げる

			# 5秒おきに'command'を送って、Telloが自動終了しないようにする
			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()    # メイン関数を実行
```

こうして，`Tello_key`のディレクトリ内には，４つのファイルがあるはずです．

```bash:ファイルは4つ
$ ls
kbhit.py  libh264decoder.so  main.py  tello.py
```



##プログラムの実行

「Tello_battery」の時と同様に，プログラム本体はmain.pyなので，コマンドラインから実行します．

```python:プログラムの実行
$ python main.py
```
ctrl+cを押すことで，プログラムを終了できます．

操作は，<font color="blue">**キーをポンっと1回叩き，次のキーを押すまでは2秒ほど待つようにしてください**</font>．FPSゲームの様な感覚で<font color="red">**キーを押しっぱなしにするのは厳禁です！**</font>

`up,down,left,right,forward,back,cw,ccw`のコマンドは，完了するまで数秒かかるので，キーを押しっぱなしにしていると先行入力が貯まって暴走します．

１コマンドずつ動作確認するつもりでキーを打ってください．

##実行結果

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

```bash:実行結果
$ python main.py
sent: command
sent: streamon
[h264 @ 0x25db7a0] non-existing PPS 0 referenced
[h264 @ 0x25db7a0] non-existing PPS 0 referenced
[h264 @ 0x25db7a0] decode_slice_header error
[h264 @ 0x25db7a0] no frame!
[h264 @ 0x25db7a0] non-existing PPS 0 referenced
[h264 @ 0x25db7a0] non-existing PPS 0 referenced
[h264 @ 0x25db7a0] decode_slice_header error
[h264 @ 0x25db7a0] no frame!
>> send cmd: command
>> send cmd: takeoff      ←ここでtキーをおした
>> send cmd: command
[h264 @ 0x25db7a0] concealing 1670 DC, 1670 AC, 1670 MV errors in P frame
>> send cmd: right 30      ←ここでdキーをおした
>> send cmd: command
>> send cmd: right 30      ←ここでdキーをおした
>> send cmd: down 30      ←ここでfキーをおした
>> send cmd: command
>> send cmd: land      ←ここでlキーをおした
>> send cmd: command
SIGINTを検知
```

5秒おきに`command`が送信されているのがわかります．
Telloは15秒間コマンドがないと自動着陸するので，それを回避する機能が効いています．

#main.pyの解説

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

##shebangと文字コード指定

前回と同様にshebangと文字コード指定が書いてあります．

```python:shebangと文字コード
#!/usr/bin/env python
# -*- coding: utf-8 -*-
```
この２行は，「おまじない」として毎回書きましょう．

##import部分

tello.pyのTelloクラスに加えて，kbhitを使いたいので，`from kbhit import *`が書かれています．
こう書くことで，kbhit.pyの中にあるkbhitやgetchなどの全ての関数(`*`アスタリスクが全てを意味している)を使うことができるようになります．

```bash:インポート
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
from kbhit import *	# kbhit.pyをインポート
```

##メイン関数

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

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

    ループ部

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

###初期化部


```python:初期化処理部
	# kbhitのためのおまじない
	atexit.register(set_normal_term)
	set_curses_term()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	drone = tello.Tello('', 8889)		# タイムアウト時間を短くし，なるべく早くキー入力に対応できるようにした

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ

	current_time = time.time()	# 現在時刻の保存変数
	pre_time = current_time		# 5秒ごとの'command'送信のための時刻変数
```
kbhitを使うためには，プログラム開始時に2つの関数を書いておく必要があります．
これは細かいことを考えず，「おまじない」として書くものだと思っておいたほうが良いです．
（中身を理解するのはPythonに精通してから）

Telloクラスを元にdroneインスタンスを作っているのは，前回と同様です．

Telloとの通信が安定するのを待つ意味で，`time.sleep`で0.5秒ほど待っています．

`current_time(現在時刻)`と`pre_time(前回時刻)`は，前回と現在との時間差を計算するために必要な変数です．ループ部でこの変数を使うときには異なる値が入るのですが，初期化部の段階では両方共に同じ値を入れています．

###ループ部

`while True`で永久ループを作っています．
ctrl+cを検知してループを終了させるのは`try except`にお任せです．


```python:ループ部

	#Ctrl+cが押されるまでループ
	try:
		while True:
			if kbhit():     # 何かキーが押されるのを待つ
				key = getch()   # 1文字取得

				# キーに応じた処理
				if key == 't':		# 離陸
					drone.takeoff()
				elif key == 'l':	# 着陸
					drone.land()
				(略)

			time.sleep(0.3)	# 適度にウェイトを入れてCPU負荷を下げる

			# 5秒おきに'command'を送って、Telloが自動終了しないようにする
			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を検知" )
```
C言語と同様に，`kbhit()`でキーボード入力の有無，`key = getch()`で入力されたキーのキーコードを取り出します．`if`文で，キーが押された時の処理を分岐させています．
Pythonには`switch`文は無いので，`if 〜　elif(else ifの意味) 〜 elif 〜`の様に書くしかありません．

Telloクラスの中には，`takeoff()`や`land()`，`move_なんちゃら`という移動に関する関数が予め用意されています（詳しくはtello.pyのソースを読むこと）．ユーザーは`drone.takeoff()`の様にドットを付けて呼びだすだけです．

またループの最後に，現在時刻`current_time`と，前回時刻`pre_time`との差を調べて，5秒以上だったらTelloに'command'を送信しています．
Telloは15秒間コマンドが来ないと自動着陸して終了してしまうので，キー入力が無くても5秒おきにコマンドを出す様にしてあるのです．
コマンドを送信した時刻が`pre_time`になるので，drone.send_command('command')と一緒に`pre_time = current_time`を書いて時刻を更新しています．

###終了処理部
終了処理も簡単に1行です．
クラスを削除しているだけです．

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

#おわりに

キーボード入力でTelloを操縦できるようにしました．
とは言え，キーを１個１個押していく感じなので，リアルタイムに操縦している感じではありませんね．

次回は，ジョイパッド/ジョイスティックを使って，ほぼリアルタイムにTelloを動かしてみようと思います．
(FPSの様にキーボードでリアルタイムに動かすのは，次のジョイスティックをやれば作れるようになります)