#プログラミング ROS< key_to_twist >

はじめに

1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第14弾として，key_to_twistを扱う．

環境

仮想環境


ソフト	VMware Workstation 15
実装RAM	2 GB
OS	Ubuntu 64 ビット
isoファイル	ubuntu-mate-20.04.1-desktop-amd64.iso

コンピュータ


デバイス	MSI
プロセッサ	Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz
実装RAM	8.00 GB (7.89 GB 使用可能)
OS	Windows (Windows 10 Home, バージョン：1909)

ROS


Distribution	noetic
プログラミング言語	Python 3.8.5

ロボットの操作

遠隔操作や自律システムなどがあるが，遠隔操作のほうが簡単であるため遠隔操作の学習から始める．ここで扱う遠隔操作はキー入力を用いるTeleop-botというものである．

Teleop-botの速度コマンド

ロボットを操作するにあたって，速度コマンドをロボットに伝えなければならない．そのために2つのプログラムにより構成する．
<プログラム①>　　（学習済み https://qiita.com/Yuya-Shimizu/items/98e9b9456b7e1945c941 ）
キー入力を受け取り，それをメッセージとして配信する

<プログラム②>
①のメッセージを受け取り，レスポンスとしてTwistメッセージを出す

実装

今回は前回に引き続き，プログラム②，すなわちkey_to_twistの実装を行う．以下にソースコードとそのときの実行の様子を示す．

ソースコード

key_to_twist.py

# ! /usr/bin/env python3

"""keysを購読し，twistを配信する"""

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

# キーの割当：[angular.z, linear.x]
key_mapping = {'w':[ 0, 1], 'x':[0, -1],
			   'a':[-1, 0], 'd':[1,  0],
			   's':[ 0, 0]}

def keys_callback(msg, twist_pub):
	if len(msg.data) == 0 or msg.data[0] not in key_mapping.keys():
		return	#データがないもしくはキーマッピングにないデータの場合，何もせずに終了
	
	velocity = key_mapping[msg.data[0]]	#キーマッピングからキーに合わせて抽出
	t = Twist() 	#Twistインスタンス生成

	t.angular.z, t.linear.x = velocity	#配列の要素をそれぞれz, xに代入

	twist_pub.publish(t)	#トピック配信

if __name__ == '__main__':
	rospy.init_node('keys_to_twist')	#ノードの初期化
	twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)	#cmd_vel配信準備
	rospy.Subscriber('keys', String, keys_callback, twist_pub)	#keysを購読し，コールバック関数を呼び出す．引数はさらに後ろで指定する

	rospy.spin()	#ループcmd_vel

実行様子

なお，この上記のプログラムでは，入力から出力までの周期が乱れてしまう可能性がある．（指定していないから）そこで，次はrospy.Rate()により周期を毎回計算して安定した周期で実行できるプログラムを組む．以下にソースコードとそのときの実行の様子を示す．

ソースコード(安定周期)

key_to_twist_using_rate.py

# ! /usr/bin/env python3

"""rospy.Rate()を使うことで安定した周期でのループを実現"""

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

# キーの割当：[angular.z, linear.x]
key_mapping = {'w':[ 0, 1], 'x':[0, -1],
			   'a':[-1, 0], 'd':[1,  0],
			   's':[ 0, 0]}

last_twist = None	#何も押されなかった場合，直前のtwistを配信するための記録用変数

def keys_callback(msg, twist_pub):
	global last_twist

	if len(msg.data) == 0 or msg.data[0] not in key_mapping.keys():
		return	#データがないもしくはキーマッピングにないデータの場合，何もせずに終了
	
	velocity = key_mapping[msg.data[0]]	#キーマッピングからキーに合わせて抽出
	last_twist = Twist() 	#Twistインスタンス生成(0に初期化)
	last_twist.angular.z, last_twist.linear.x = velocity	#配列の要素をそれぞれz, xに代入
	twist_pub.publish(last_twist)	#トピック配信

if __name__ == '__main__':
	rospy.init_node('keys_to_twist')	#ノードの初期化
	twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)	#cmd_vel配信準備
	rospy.Subscriber('keys', String, keys_callback, twist_pub)	#keysを購読し，コールバック関数を呼び出す．引数はさらに後ろで指定する

	rate = rospy.Rate(10)	#10Hz(100ミリ秒ごと)で出力するため
	last_twist = Twist()	#0に初期化

	while not rospy.is_shutdown():
		twist_pub.publish(last_twist)
		rate.sleep()

実行様子

より挙動が分かるようにgif形式で動画像として示している．

上図からも分かるように，ここでは，入力がなかった場合は直前状態を維持した速度コマンドが配信されるようになっている．

また，rqt_plotを使うことで，その速度コマンドの変化の様子をグラフで確認することができる．そのときの様子を次に示す．

rqt_plot

注意点

ロボットの用途により，要求される速度が異なる．今回の秒速1mというのも自動運転車には遅いが，屋内用ロボットにとってはきわめて速い．このプログラムが複数のロボットを扱えるようになるためには，このプログラムにパラメータ化できる機能が必要となる．これは，次回学習することとする．

感想

いよいよ速度コマンドも扱えるようになり本格的になってきた．また，rqt_plotによりグラフからその出力の様子を知ることができるのも直感的に分かりやすく強力なツールであると思えた．次回は複数のロボットにプログラムを汎用的に適用するために必要なパラメータ化を学ぶということで楽しみである．

参考文献

プログラミングROS Pythonによるロボットアプリケーション開発
　　　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart 著
　　　　　　　　　　　　　　　　　　　　　　　河田　卓志　監訳
　　　　　　　　　　　松田　晃一，福地　正樹，由谷　哲夫　訳
　　　　　　　　　　　　　　　　　　オイラリー・ジャパン　発行