Help us understand the problem. What is going on with this article?

キー入力操縦プログラム

はじめに

このページは,

公式SDK「Tello-Python」を試そう

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

概要

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

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

前提条件

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

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

詳しくは Tello-Pythonのダウンロード を御覧ください.

Telloをキーボードで操縦するプログラム

ディレクトリの作成

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

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

ファイルをコピー

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

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

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

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

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

kbhit.pyについては, dronekitの記事 でも紹介しています.

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

   kbhit.py

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

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

from kbhit import *     

(2) プログラムの冒頭(インポートの後)に,この2行を書く

atexit.register(set_normal_term)
set_curses_term()

(3) 永久ループ内でキー入力があるかどうかチェック(C言語と同じ書き方)

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

まさにkbhitgetch!,古い人間にはこれで十分です(爆

Tello操作のキー配置

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

というわけで,今回作成するプログラムは,下図の様な操作系にしようと思います.
key_control.png

TakeoffのtとLandのlだけ,頭文字のキーにしています.

main.py

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

以下のコードをコピー&ペーストするか,
ここ を右クリックして[名前を付けて保存]機能でファイル保存してください.

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のディレクトリ内には,4つのファイルがあるはずです.

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

プログラムの実行

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

プログラムの実行
$ python main.py

ctrl+cを押すことで,プログラムを終了できます.

操作は,キーをポンっと1回叩き,次のキーを押すまでは2秒ほど待つようにしてください.FPSゲームの様な感覚でキーを押しっぱなしにするのは厳禁です!

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

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

実行結果

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

実行結果
$ 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と文字コード指定が書いてあります.

shebangと文字コード
#!/usr/bin/env python
# -*- coding: utf-8 -*-

この2行は,「おまじない」として毎回書きましょう.

import部分

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

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

メイン関数

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

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

    ループ部

    終了処理部

それぞれ解説していきます.

初期化部

初期化処理部
    # 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にお任せです.

ループ部
    #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行です.
クラスを削除しているだけです.

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

おわりに

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

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

Why do not you register as a user and use Qiita more conveniently?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away