Go to Qiita Advent Calendar 2024 Top
LoginSignup
24
23

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@hsgucci

ジョイスティック操縦プログラム

Last updated at Posted at 2019-10-04

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

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

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

#概要

前回の記事では，tello.pyのTelloクラスを使って，Telloをキーボードで操縦するプログラムを作りました．
しかし，キーを押しっぱなしにして操作することはできない仕様だったので，リアルタイムにラジコンを操縦する感覚ではありませんでした．

本企画は「OpenCVの画像処理を用いてTelloを自律ロボット化する」のが本筋なのですが，今回はちょっと寄り道してジョイパッド/ジョイスティックでTelloを操縦してみます．
（「Tello-Pythonでどこまでできるのか，可能性を見せる」という感じ？）

#前提条件

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

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

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

#ジョイスティックを準備しよう

アナログ入力が少なくとも4軸あるジョイパッドまたはジョイスティックを用意しましょう．
プレイステーションのコントローラの様に，２つのアナログスティックが付いていればＯＫです．

USB接続の製品であれば，Linuxでも問題なく認識してくれると思います．
Bluetooth接続は，製品に依存するのでオススメできません．(それぞれの製品をLinuxで使う，的な記事を探して設定する必要があります．)

この記事では，古いデバイスをたくさん引っ張り出してきて遊びました(笑

  • ロジクール F710
  • プレイステーション３のコントローラ(DualShock3)をUSB接続
  • プレイステーション４のコントローラ(DualShock4)をUSB接続
  • XBOXコントローラそっくりの中国製の安いやつ
  • サンワサプライの古いジョイパッド
  • エレコムの古いジョイパッド
  • Madcatz Cyborg F.L.Y.5
  • Saitek X52 Pro
  • Thrustmaster HOTAS couger
  • Thrustmaster Top Gun Afterburner

いつか鉄騎コントローラでTelloを操縦したいなあ．．．

#Telloをジョイスティックで操縦するプログラム

##ディレクトリの作成

まずは，Tello-Pythonディレクトリの下に，新しいディレクトリTello-joyを作ります．
(前回のTello-keyに名前が似ているので注意)

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

##ファイルをコピー

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

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

##ジョイスティックを管理するツール

ジョイパッドやジョイスティックは，製品に応じてボタン番号やアナログ軸番号が異なるので，プログラムするまえに動作を確認する必要があります．
そんな時に便利なのがjstest-gtkです．

インストールは以下のコマンドです．

jstest-gtkのインストール
$ sudo apt install jstest-gtk

ちなみにRaspberry Piでも問題なく使えるアプリです．

下図のように，Linuxのスタートメニューに「jstest-gtk」が追加されているはずです．
jstest-menu.png
アプリを起動すると，ＰＣに接続されているジョイスティックを列挙した画面が表示されます．
jstest-gtk.png
調べたいジョイスティックをダブルクリックすると，アナログスティックやボタンの情報を見ることができます．
jstest-joy.png
GUIのウィンドウアプリケーションなので，お手軽に使えます．
Windowsの[ゲームコントローラの設定]みたいな機能です．

jstest-gtkには，番号順序を変更（リマップ）したり，アナログ軸の方向を反転(リバース)させたりする機能もありますが，今回は使いません．
システム側でイジるよりも，プログラム側で対応することに慣れておいたほうが今後のためにも良いでしょう．

##Pythonでジョイスティック入力を取る方法

Pythonでジョイスティック入力を取る方法はいくつかありますが，今回はpygameというライブラリを使います．

pygameは，Pythonでグラフィカルなゲームを作るときに使うライブラリで，GUIウィンドウの生成や描画，サウンド，入出力などの多彩な機能を持っています．ですが今回は，pygameのジョイスティックを読む機能だけを使います．

Raspberry Piだとpygameはデフォルトで入っているのですが、普通のPC Linuxだと入っていない事もあります。
以下のコマンドでインストールします．

pygameのインストール
$ sudo pip2 install pygame

Linuxの環境によっては、sudoを付ける必要があったり、pipをpip2と書いたりする必要があります。

#Tello操作のキー配置

##ジョイパッドの場合
ジョイパッドのスティックの使い方は，ラジコンのモード１と同じ配置にしました．
というか，筆者はモード１しか飛ばせないので(T_T
mode1.png
モード２の人は，適宜プログラムを変えてください．

##ジョイスティックの場合
ジョイスティックの場合は，実機ヘリコプターと同様のイメージにしました．
stick.png
右スティックがサイクリックピッチ(前後左右移動)，
左のスロットルレバーがコレクティブピッチ(上昇下降)，のつもりです．
フットペダルが左右旋回なのですが，ペダルはオプションなので所有していない人のほうが多いです．その場合は「左レバーに付いているアナログ軸」あるいは「右スティックの『ねじり』軸」などに割り当てると良いでしょう．
こうして見ると，プロポのモード２というのは実機ヘリに近いことがわかりますね．
　　「プロポ モード２」で画像検索

##Tello SDKのラジコン操作コマンド

前回のキー入力と同様に，forward,back,left,rightなどのコマンドを使っても良いのですが，これらの移動コマンドは応答が遅いです．

Tello SDKのPDFをよく見ると，ラジコンの様にスティックの状態を流し込めるコマンドがあります．

・Tello SDK 1.3
・Tello SDK 2.0

コマンド名 解説 備考
rc a b c d 4チャンネル分のリモコン操作をセット “a” = left/right (-100〜100)
“b” = forward/backward (-100〜100)
“c” = up/down (-100〜100)
“d” = yaw (-100〜100)

このコマンドを使うには，Telloクラスのsend_command関数を利用して，以下の様に書きます．

send_commandを使ってrcコマンドを実装
drone.send_command('rc %s %s %s %s'%(a, b, c, d) )   # a,b,c,dには±100以内の操作量を入れる

a,b,c,dという変数には，ジョイスティックの値を入れてあげれば良いわけです．
ただし，数値は±100の範囲内に収める必要があります．

##main.py

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

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

main.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import pygame		# pygameでジョイスティックを読む

def main():
	# pygameの初期化とジョイスティックの初期化
	pygame.init()
	joy = pygame.joystick.Joystick(0)	# ジョイスティック番号はjstest-gtkで確認しておく
	joy.init()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	#   コマンドの応答タイムアウトを0.01秒(10ms)にして，rcコマンドの連送に耐えられるようにする
	drone = tello.Tello('', 8889, command_timeout=.01 )

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ
	# pygameの初期化とジョイスティックの初期化
	pygame.init()
	joy = pygame.joystick.Joystick(0)	# ジョイスティック番号はjstest-gtkで確認しておく
	joy.init()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	#   コマンドの応答タイムアウトを0.01秒(10ms)にして，rcコマンドの連送に耐えられるようにする
	drone = tello.Tello('', 8889, command_timeout=.01 )

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

	#Ctrl+cが押されるまでループ
	try:
		while True:
			# Joystickの読み込み
			#   get_axisは　-1.0〜0.0〜+1.0 で変化するので100倍して±100にする
			#   プラスマイナスの方向が逆の場合は-100倍して反転させる
			a = int( joy.get_axis(2)*100 )		# aは左右移動
			b = int( joy.get_axis(1)*-100 )		# bは前後移動
			c = int( joy.get_axis(3)*-100 )		# cは上下移動
			d = int( joy.get_axis(0)*100 )		# dは旋回
			btn0 = joy.get_button(0)
			btn1 = joy.get_button(1)
			btn2 = joy.get_button(2)
			btn3 = joy.get_button(3)
			pygame.event.pump()		# イベントの更新

			# プラスマイナスの方向や離陸/着陸に使うボタンを確認するためのprint文
			#print("l/r=%d  f/b=%d  u/d=%d  cw/ccw=%d  btn0=%d  btn1=%d  btn2=%d  btn3=%d"%(a, b, c, d, btn0, btn1, btn2, btn3))

			# rcコマンドを送信
			drone.send_command( 'rc %s %s %s %s'%(a, b, c, d) )

			if btn1 == 1:		# 離陸
				drone.takeoff()
			elif btn2 == 1:		# 着陸
				drone.land()

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

	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を押すことで，プログラムを終了できます．

##実行結果

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

実行結果
$ python main.py
pygame 1.9.6
Hello from the pygame community. https://www.pygame.org/contribute.html
sent: command
sent: streamon
[h264 @ 0x195ba40] non-existing PPS 0 referenced
[h264 @ 0x195ba40] non-existing PPS 0 referenced
[h264 @ 0x195ba40] decode_slice_header error
[h264 @ 0x195ba40] no frame!
(略)
>> send cmd: rc 0 0 0 0
>> send cmd: rc 0 0 0 0
>> send cmd: rc 0 0 0 0
>> send cmd: rc 38 82 86 99
>> send cmd: rc 29 82 86 99
.
.
.

#main.pyの解説

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

##import部分

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

インポート
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import pygame		# pygameでジョイスティックを読む

pygameを読み込んでいます。

##メイン関数

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

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

    ループ部

    終了処理部

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

###初期化部

初期化処理部
	# pygameの初期化とジョイスティックの初期化
	pygame.init()
	joy = pygame.joystick.Joystick(0)	# ジョイスティック番号はjstest-gtkで確認しておく
	joy.init()

	# Telloクラスを使って，droneというインスタンス(実体)を作る
	#   コマンドの応答タイムアウトを0.01秒(10ms)にして，rcコマンドの連送に耐えられるようにする
	drone = tello.Tello('', 8889, command_timeout=.01 )

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

pygameの初期化とジョイスティックのインスタンス作成，ジョイスティックの初期化をしています．

今回はTelloクラスの呼び出し時の引数にcommand_timeout=.01を追加してあります．
これはtakeoffなどのコマンドに対するTelloの応答を待つ時間「タイムアウト時間」の指定です．
デフォルト値は0.3なので300ミリ秒の待ちですが，0.01で10ミリ秒しか待たないようにしました．
こうすることで，ラジコンのプロポでドローンを動かしている様な感覚で操縦できるようになります．

また，前回のキー入力で操縦の際に行った「５秒おきにcommandを送って，Telloが止まるのを回避する」機能はありません．というのは，ジョイスティックの値をrcコマンドで常時タレ流しにするので，コマンドが来なくなる状況には成り得ないからです．

###ループ部

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

ループ部
	#Ctrl+cが押されるまでループ
	try:
		while True:
			# Joystickの読み込み
			#   get_axisは　-1.0〜0.0〜+1.0 で変化するので100倍して±100にする
			#   プラスマイナスの方向が逆の場合は-100倍して反転させる
			a = int( joy.get_axis(2)*100 )		# aは左右移動
			b = int( joy.get_axis(1)*-100 )		# bは前後移動
			c = int( joy.get_axis(3)*-100 )		# cは上下移動
			d = int( joy.get_axis(0)*100 )		# dは旋回
			btn0 = joy.get_button(0)
			btn1 = joy.get_button(1)
			btn2 = joy.get_button(2)
			btn3 = joy.get_button(3)
			pygame.event.pump()		# イベントの更新

			# プラスマイナスの方向や離陸/着陸に使うボタンを確認するためのprint文
			#print("l/r=%d  f/b=%d  u/d=%d  cw/ccw=%d  btn0=%d  btn1=%d  btn2=%d  btn3=%d"%(a, b, c, d, btn0, btn1, btn2, btn3))

			# rcコマンドを送信
			drone.send_command( 'rc %s %s %s %s'%(a, b, c, d) )

			if btn1 == 1:		# 離陸
				drone.takeoff()
			elif btn2 == 1:		# 着陸
				drone.land()

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

	except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
		print( "SIGINTを検知" )

joyクラスにはget_axis(軸番号)とget_button(ボタン番号)で値を取るメンバがあります．

rc a b c dコマンドのa,b,c,dに対応する軸番号が何番なのかをprint文で探しました．プラスマイナスの方向もこのprint文で確認します．また，ボタン番号も確認して，離陸/着陸に使いたいボタンがどれかを調べました．
必要な情報を集めたら，print文はコメントアウトします．

drone.send_command( 'rc %s %s %s %s'%(a, b, c, d) )でジョイスティックの値を送信しています．

今回はボタン1と2が離陸と着陸だったので，if文でボタンが押された時の処理を書いてあります．

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

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

#おわりに

今回はちょっと脇道にそれてジョイスティックでTelloを操縦しました．

Telloをプログラムで動かすには，

  • forward,back,left,right,up,downで移動距離を指定する方法
  • rcで，直接移動量を指定する方法

の２種類があることが分かりました．
画像処理のプログラムでTelloを動かすときには，目的に応じてどちらを使うべきなのか考えると良いでしょう．

次回は，本筋に戻って画像処理の入門をやります．

24
23
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
24
23

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?