#プログラミング ROS< 速度の増減 >

はじめに

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

環境

仮想環境


ソフト	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

物理的な問題

前回で，ロボットの最大速度を指定することができるようにはなったが，有限の加速度という物理的な問題を扱う必要が出てくる．

質量を持った多くの物体と同様に，ロボットも瞬時にスタートしたり，ストップしたりはできない．ロボットの移動を物理学でとらえると，時間とともにだんだん加速していく物体である．急に止めたりしようとすると，モータに急激な負荷をかけることになる．例えば，横滑り，スリップ，振動，メカニカルな駆動系で何か壊れるといった問題に発展し得る．これを避けるために，ある一定の時間内で移動コマンドを増減させる．

実装

先ほどのことを踏まえて，要求しても瞬間的に加速するのを防ぐプログラムを学ぶ．以下にソースコードとそのときの実行様子を示す．

ソースコード

keys_to_twist_with_ramps.py

# ! /usr/bin/env python3

"""瞬間的に加速するのを防ぐために速度を一定時間で増減させる"""

import rospy
import math
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]}

twist_pub      = None		#twist配信用の変数
target_twist   = None		#目標の速度インスタンス用変数
last_twist     = None		#何も押されなかった場合，直前のtwistを配信するための記録用変数
last_send_time = None		#配信した時刻を記録する変数
vel_scales     = [0.1, 0.1]	#デフォルトの速度（非常に遅い）
vel_ramps      = [1, 1]		#単位はm/s　増減の度合い 

def ramped_vel(vel_prev, vel_target, time_prev, time_now, ramp_rate):
	"""	最大の速度ステップを計算する """

	step = ramp_rate * (time_now - time_prev).to_sec()
	
	if vel_target > vel_prev:	#目標値に達していなければsignを1.0に
		sign = 1.0
	else:						#目標値に達しているならばsignを-1.0に
		sign = -1.0
	
	error = math.fabs(vel_target - vel_prev)	#絶対値計算　abs()と違って，引数がintでもfloatで返す
	
	if error < step:	#この時間ステップ内にそこに到達できる --> 到達した
		return vel_target
	else:
		return vel_prev + sign * step	#ターゲットに向けてステップを進める

def ramped_twist(prev, target, time_prev, time_now, ramps):
	"""	計算した速度ステップをtwistに適用 """
	twist = Twist()
	twist.angular.z = ramped_vel(prev.angular.z, target.angular.z, time_prev, time_now, ramps[0])
	twist.linear.x  = ramped_vel(prev.linear.x, target.linear.x, time_prev, time_now, ramps[1])

	return twist

def send_twist():
	"""	twistを送る """
	global last_send_time, target_twist, last_twist, vel_scales, vel_ramps, twist_pub

	time_now = rospy.Time.now()

	last_twist = ramped_twist(last_twist, target_twist, last_send_time, time_now, vel_ramps)

	last_send_time = time_now
	twist_pub.publish(last_twist)		#twist配信

def keys_callback(msg):
	global target_twist, last_twist, vel_scales

	if len(msg.data) == 0 or msg.data[0] not in key_mapping.keys():
		return	#データがないもしくはキーマッピングにないデータの場合，何もせずに終了
	
	velocity = key_mapping[msg.data[0]]	#キーマッピングからキーに合わせて抽出
	target_twist = Twist() 	#Twistインスタンス生成(0に初期化)
	target_twist.angular.z = velocity[0] * vel_scales[0]	#配列の要素に速度スケール値をかけて目標角速度に代入
	target_twist.linear.x = velocity[1] * vel_scales[1]		#配列の要素に速度スケール値をかけて目標速度に代入

def fetch_param(name, default):
	""" コマンドラインでの入力の受付"""
	if rospy.has_param(name):	#コマンドラインでパラlast_send_timeメータnameが指定されているかチェック
		return rospy.get_param(name)	#指定された値を取得
	else:
		#パラメータが指定されておらず，デフォルト値を使うことを警告する
		rospy.logwarn(f"parameter {name} not defined. Defaulting to {default:.1f}")
		return default


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

	target_twist = Twist()	#0に初期化
	last_twist = Twist()	#0に初期化

	vel_scales[0] = fetch_param('~angular_scale', 0.1)
	vel_scales[1] = fetch_param('~linear_scale', 0.1)
	vel_ramps[0]  = fetch_param('~angular_accel', 1.0)
	vel_ramps[1]  = fetch_param('~angular_accel', 1.0)


	rate = rospy.Rate(20)	#10Hz(100ミリ秒ごと)で出力するため
	while not rospy.is_shutdown():
		send_twist()
		rate.sleep()

実行様子

結果

グラフから確かに，ステップ状に変化するのではなく，だんだん増えたり減ったりしていることがわかる．加速と減速に一定の時間をかけているため，物理的に実行可能であるということも確認できた．

感想

今回は少しソースコードも関数が増えて複雑になったが，前回に危惧されたステップコマンドによる物理的問題を解消する方法を学んだ．特にこれからロボットを扱っていく中で，非常に重要で意識しておくべきことだと思う．

次回はシミュレーション上で，実際にturtlebotを動かしてみる．

参考文献

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