ROSの勉強 第25弾：ステートマシン（巡回）

#プログラミング ROS< ステートマシン（巡回） >

はじめに

1つの参考書に沿って，ROS(Robot Operating System)を難なく扱えるようになることが目的である．その第25弾として，「ステートマシン（巡回）」を扱う．

環境

仮想環境

ソフト	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, バージョン：20H2)

ROS

Distribution	noetic
プログラミング言語	Python 3.8.5
シミュレーション	gazebo

ステートマシン（State Machine）

概要

名前のとおり，ある状態を表す言葉であるが，ステートマシンとは，次のような考え方である．
「ロボットは有限の数の状態（ステート）群，すなわち（待機:waiting）・（移動:moving）・（充電:charging）など，振る舞いと対応付けられたステートのうち1つのステートにいる」
ステートマシンはきわめて複雑な振る舞いの制御にも使うことができる．

「待機」「移動」「充電」といったステートに対応する振る舞いは，それぞれのステートの中にカプセル化され，ステート間の遷移はステートマシンの構造によって制御される．

カプセル化: https://qiita.com/Yuya-Shimizu/items/7c86729308bcaea602ca

ROSのステートマシン

ROSの命名規則により，ステートはすべて大文字で，遷移条件（successなど）はすべて小文字で名前を付ける．

ROSのステートマシンは，smachパッケージとそのROS専用の拡張機能であるsmach_rosで構成される．

実装

以下では，実装しながら，ステートマシンについて学び，最後にはそれをロボットに応用する．

smachでステートマシンを定義

タスク1

ONEステートは「one」という文字を出力して，TWOステートは「two」という文字を出力し，それらのステートを切り替えていく．

ソースコード

simple_sm.py

# ! /usr/bin/env python3

import rospy
from smach import State, StateMachine

from time import sleep

class One(State):
	def __init__(self):
		State.__init__(self, outcomes = ['success'])

	#各ステートにexecute関数は必須で，ステートのすべての処理がこの中で行われる
	def execute(self, userdata):
		print('one')
		sleep(1)
		return 'success'

class Two(State):
	def __init__(self):
		State.__init__(self, outcomes = ['success'])

	#各ステートにexecute関数は必須で，ステートのすべての処理がこの中で行われる
	def execute(self, userdata):
		print('two')
		sleep(1)
		return 'success'

if __name__ == '__main__':
	#ステートマシンの作成
	#smにStateMachineのインスタンスを作成する：引数には取りうる出力結果のリストを渡す
	sm = StateMachine(outcomes=['success'])

	#今作った空のステートマシンにステートを追加する
	with sm:
		StateMachine.add('ONE', One(), transitions={'success':'TWO'})
		StateMachine.add('TWO', Two(), transitions={'success':'ONE'})

	sm.execute()	#この関数を実行することで，初期状態（ステート）からスタートする

実行様子

[DEBUG]部分でステートマシンの構築が行われていることが分かる．
その後，[INFO]を見ると，ONEステートから始めて遷移し続けていることが確認できる．

タスク2

直進と回転を組み合わせて多角形の経路を描くことを想定したメッセージを出力する．ここでのステートは直進（距離）と回転（角度）だが，描く多角形に応じてそのステートの数は異なる．

ソースコード

shapes.py

# ! /usr/bin/env python3

"""
直進と回転を組み合わせることで，
多角形の経路に沿ってロボットを動かすことを想定している
"""

import rospy
from smach import State, StateMachine

from time import sleep

class Drive(State):
	"""直進クラス"""
	def __init__(self, distance):	#移動量を指定できるように引数を追加
		State.__init__(self, outcomes=['success'])
		self.distance = distance

	def execute(self, userdata):
		print('Driving', self.distance)
		sleep(1)
		return 'success'

class Turn(State):
	"""回転クラス"""
	def __init__(self, angle):	#回転量を指定できるように引数を追加
		State.__init__(self, outcomes=['success'])
		self.angle = angle

	def execute(self, userdata):
		print('Turning', self.angle)
		sleep(1)
		return 'success'

if __name__ == '__main__':
	#三角形を描くステートマシン
	triangle = StateMachine(outcomes=['success'])

	with triangle:
		StateMachine.add('SIDE1', Drive(1), transitions={'success':'TURN1'})
		StateMachine.add('TURN1', Drive(120), transitions={'success':'SIDE2'})
		StateMachine.add('SIDE2', Drive(1), transitions={'success':'TURN2'})
		StateMachine.add('TURN2', Drive(120), transitions={'success':'SIDE3'})
		StateMachine.add('SIDE3', Drive(1), transitions={'success':'success'})	#'success'を指定することで終了


	#正方形を描くステートマシン
	square = StateMachine(outcomes=['success'])

	with square:
		StateMachine.add('SIDE1', Drive(1), transitions={'success':'TURN1'})
		StateMachine.add('TURN1', Drive(90), transitions={'success':'SIDE2'})
		StateMachine.add('SIDE2', Drive(1), transitions={'success':'TURN2'})
		StateMachine.add('TURN2', Drive(90), transitions={'success':'SIDE3'})
		StateMachine.add('SIDE3', Drive(1), transitions={'success':'TURN3'})
		StateMachine.add('TURN3', Drive(90), transitions={'success':'SIDE4'})
		StateMachine.add('SIDE4', Drive(1), transitions={'success':'success'})	#'success'を指定することで終了	#'success'を指定することで終了


	#先程のステートマシンをさらに1つのステートマシンに統合
	shapes = StateMachine(outcomes=['success'])

	with shapes:
		StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'})
		StateMachine.add('SQUARE', square, transitions={'success':'success'})	#'success'を指定することで終了


	shapes.execute()

実行様子

StateMachineの遷移を示すところで，{'success':'success'}とすることで，遷移せずに終了することができる．

もう少しうまくステートマシンを定義

タスク3

タスク2と同じだが，プログラムの書き方だけが異なる．ステートマシンの定義には規則的な部分があるため，for文などを利用してまとめて書くこともできる．また，多角形についてももう少しうまく定義することができる．

ソースコード

shapes2.py

# ! /usr/bin/env python3

"""
直進と回転を組み合わせることで，
多角形の経路に沿ってロボットを動かすことを想定している
"""

import rospy
from smach import State, StateMachine

from time import sleep

class Drive(State):
	"""直進クラス"""
	def __init__(self, distance):	#移動量を指定できるように引数を追加
		State.__init__(self, outcomes=['success'])
		self.distance = distance

	def execute(self, userdata):
		print('Driving', self.distance)
		sleep(1)
		return 'success'

class Turn(State):
	"""回転クラス"""
	def __init__(self, angle):	#回転量を指定できるように引数を追加
		State.__init__(self, outcomes=['success'])
		self.angle = angle

	def execute(self, userdata):
		print('Turning', self.angle)
		sleep(1)
		return 'success'

# 多角形のステートマシンを生成する関数の定義
def polygon(sides):
	"""n角形のnを与えることで，そのステートマシンをつくる"""

	polygon = StateMachine(outcomes=['success'])
	
	with polygon:
		#最後の直進以外を追加
		for i in range(sides-1):
			StateMachine.add(f"SIDES{i+1}", Drive(1), transitions={'success':f"TURN{i+1}"})
		#すべての回転を追加
		for i in range(sides-1):
			StateMachine.add(f"TURN{i+1}", Turn(360/sides), transitions={'success':f"SIDES{i+2}"})
		#最後の直線を追加
		StateMachine.add(f"SIDES{sides}", Drive(1), transitions={'success':'success'})
	
	return polygon


if __name__ == '__main__':
	#三角形を描くステートマシン
	triangle = polygon(3)

	#正方形を描くステートマシン
	square = polygon(4)

	#先程のステートマシンをさらに1つのステートマシンに統合
	shapes = StateMachine(outcomes=['success'])

	with shapes:
		StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'})
		StateMachine.add('SQUARE', square, transitions={'success':'success'})	#'success'を指定することで終了

	shapes.execute()

実行様子

ステートマシンで巡回

タスク4

2か所の経由ポイントを巡回する．（片方到着後．次のステートへ）
なお，経由ポイントを変更することも可能で，経由ポイントの数を増やすことも可能である．その際は，ソースコードのwaypointsの変更身で実行可能となる．

ソースコード

patrol_sm.py

# ! /usr/bin/env python3

import rospy
import actionlib
from smach import State, StateMachine
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

waypoints = [
			['one', (-2.0, 0.8), (0.0, 0.0, 0.0, 1.0)],
			['two', (-2.0, -0.8), (0.0, 0.0, 0.5, 1.0)]
			]

class Waypoint(State):
	def __init__(self, position, orientation):
		State.__init__(self, outcomes=['success'])

		#アクションクライアントを取得する
		self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
		self.client.wait_for_server()

		#ゴールを定義する
		self.goal = MoveBaseGoal()
		self.goal.target_pose.header.frame_id = 'map'
		self.goal.target_pose.pose.position.x = position[0]
		self.goal.target_pose.pose.position.y = position[1]
		self.goal.target_pose.pose.position.z = 0.0
		self.goal.target_pose.pose.orientation.x = orientation[0]
		self.goal.target_pose.pose.orientation.y = orientation[1]
		self.goal.target_pose.pose.orientation.z = orientation[2]
		self.goal.target_pose.pose.orientation.w = orientation[3]
		
	def execute(self, userdata):
		self.client.send_goal(self.goal)
		self.client.wait_for_result()
		return 'success'

if __name__ == '__main__':
	rospy.init_node('patrol')

	patrol = StateMachine('success')
	with patrol:
		for i, w in enumerate(waypoints):
			StateMachine.add(w[0], Waypoint(w[1], w[2]), transitions={'success':waypoints[(i+1)%len(waypoints)][0]})
			#transitionは奇数%2=1と偶数%2=0というように0と1を繰り返して，ステートの切り替えをしている

	patrol.execute()

実行様子

ターミナル（端末）

ロボット

タスク5

タスク4と同じだが，smachではなく，smach_rosを利用することで，ステートマシンの生成を簡素化し，ソースコード全体を簡素化することができる．

ソースコード

better_patrol_sm.py

# ! /usr/bin/env python3

import rospy
from smach import StateMachine
from smach_ros import SimpleActionState
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

waypoints = [
			['one', (-2.0, 0.8), (0.0, 0.0, 0.0, 1.0)],
			['two', (-2.0, -0.8), (0.0, 0.0, 0.5, 1.0)]
			]

"""
class Waypoint(State):
	def __init__(self, position, orientation):
		State.__init__(self, outcomes=['success'])

		#アクションクライアントを取得する
		self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
		self.client.wait_for_server()

		
	def execute(self, userdata):
		self.client.send_goal(self.goal)
		self.client.wait_for_result()
		return 'success'
"""

if __name__ == '__main__':
	rospy.init_node('patrol')

	patrol = StateMachine(['succeeded', 'aborted', 'preempted'])
	with patrol:
		for i, w in enumerate(waypoints):
			#ゴールを定義する
			goal_pose = MoveBaseGoal()
			goal_pose.target_pose.header.frame_id = 'map'
			goal_pose.target_pose.pose.position.x = w[1][0]
			goal_pose.target_pose.pose.position.y = w[1][1]
			goal_pose.target_pose.pose.position.z = 0.0
			goal_pose.target_pose.pose.orientation.x = w[2][0]
			goal_pose.target_pose.pose.orientation.y = w[2][1]
			goal_pose.target_pose.pose.orientation.z = w[2][2]
			goal_pose.target_pose.pose.orientation.w = w[2][3]

			StateMachine.add(w[0], SimpleActionState('move_base', MoveBaseAction, goal=goal_pose), transitions={'succeeded':waypoints[(i+1)%len(waypoints)][0]})
			#transitionは奇数%2=1と偶数%2=0というように0と1を繰り返して，ステートの切り替えをしている

	patrol.execute()

ActionClientの部分が必要でなくなっている．

実行様子

ターミナル（端末）

ロボット

##### launchファイル タスク4とタスク5については，少し実行するものも多かったため，launchファイルを作成してみた．不慣れではあるが，とりあえず動くものはできた．

patrol.launch

<launch>
	<include file="$(find turtlebot3_gazebo)/launch/turtlebot3_world.launch"/>
	<include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>
	<include file="$(find turtlebot3_navigation)/launch/move_base.launch"/>


	<node name="map" pkg="map_server" type="map_server" args="$(find patrol)/map2.yaml"/>

	<node name="robot_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find patrol)/patrol.rviz"/>

	<node name="patrol" pkg="patrol" type="patrol_sm.py"/>
	
</launch>

とりあえず，includeタグでは，ほかのlaunchファイルを実行でき，その他rosrunなどで実行するものについては，nodeタグを使えばよいことを知った．また，map_serverとrvizについてはそれぞれマップ情報とrvizの設定を指定したかったので，argsという引数に代入している．

感想

今回は，ステートマシンという非常に大切になってくるであろう概念について学び，その例題として巡回を通して，その応用方法についても学習することができた．また，今回launchファイルの作成にも挑戦してみた．意外とできた．少し難しそうだと思っていたが，ほかのlaunchファイルを参照していると基本的なことは理解することができた．もう少し複雑なparamタグなどについてはまだまだ理解できていないが，そのあたりは今後，少しずつ学んでいけたらと思う．

余談ではあるが，asciinemaでターミナル（端末）の実行様子記録して，この記事に埋め込もうとしたら，どうもうまくいかなかった．ヒントを見ながらやってみても何も表示されなかった．今回は諦めて直接サイトに飛ばすものにした．

参考文献

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