状態機械を作る"Smach"パッケージの使い方(1)
ロボットに限らずですが、状態機械を用いることで、様々なタスクの遷移をスマートに表現することができます。
ROSの"Smach"パッケージを使えば、状態機械をPythonスクリプトによって比較的簡単に実現できます。
Smachのチュートリアルページは全部英語だし、しかも順番がグチャグチャで訳わからんことになっているので、こちらで整理しておきます。
準備
チュートリアル用パッケージの作成
cd ~/catkin_ws/src/
catkin_create_pkg smach_tutorial rospy smach smach_ros
smach_viewerの導入
Smachで作成した状態機械を視覚的に確認できる、"smach_viewer"を導入します。 ただし、ROS Indigoだと、デフォルトではsmach_viewerがうまく動かないので、簡単な改造をします。
まずは導入。
sudo apt-get install ros-indigo-smach-viewer
そして、改造します。
ROS Indigo :Cannot show Graph View on smach_viewer - ROS Answers: Open Source Q&A Forum
/opt/ros/indigo/lib/python2.7/dist-packages/xdot/xdot.pyの480行目を、以下のように書き換えます
管理者権限が必要なので、sudoでエディタを立ち上げて、書き換えてください。
# return int(self.read_code()) #元々のコード return int(float(self.read_code())) #書き換えたコード
一番シンプルな状態機械を作る
smach/Tutorials/Getting Started - ROS Wiki
サンプルコードを、先ほど作ったsmach_tutorial以下に、state_machine_simple.pyとして保存してください。
このサンプルは、状態FOOに遷移するごとにカウンタをインクリメントし、カウンタが3を超えた時に 状態機械を終了する、という動作をします。
#!/usr/bin/env python import rospy import smach import smach_ros # define state Foo class Foo(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['outcome1','outcome2']) self.counter = 0 def execute(self, userdata): rospy.loginfo('Executing state FOO') rospy.sleep(1) if self.counter < 3: self.counter += 1 return 'outcome1' else: return 'outcome2' # define state Bar class Bar(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['outcome2']) def execute(self, userdata): rospy.loginfo('Executing state BAR') rospy.sleep(1) return 'outcome2' # main def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome4', 'outcome5']) # Open the container with sm: # Add states to the container smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'}) smach.StateMachine.add('BAR', Bar(), transitions={'outcome2':'FOO'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() rospy.spin() if __name__ == '__main__': main()
やっていることは、
- 状態の作成
- 状態機械の作成
- デバッグ出力の準備
- 状態機械の実行
の順となります。
サンプルのコードを上から順に見ていきます。
状態の作成
# define state Foo class Foo(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['outcome1','outcome2']) self.counter = 0 def execute(self, userdata): rospy.loginfo('Executing state FOO') rospy.sleep(1) if self.counter < 10: self.counter += 1 return 'outcome1' else: return 'outcome2'
Fooという状態を定義しています。
initでは、outcomes=['outcome1','outcome2']
という部分で、この状態が取りうる出力を定義しています。
シングルクォーテーションで囲まれた部分が、出力となります。
一般的には、タスクの状態(success, abort, fail等)になると思います。
executeでは、この状態に遷移した際に実行する処理を書いています。 任意の処理が終わったあとは、returnで、outcomesに定義した出力結果のうちの一つを返します。
Barに関しても同様です。
状態機械の作成
# Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
状態機械を定義しています。 状態機械が取りうる最終的な出力を、outcomesとして登録しています。
# Open the container with sm: # Add states to the container smach.StateMachine.add('FOO', Foo(), transitions={'outcome1':'BAR', 'outcome2':'outcome4'})
状態機械に対して状態を登録しています。
transitionsでは、出力結果と遷移先の状態のペアを登録します。
今回の例では、状態FOOが出力outcome1を出力した時、状態BARに遷移します。 状態FOOが出力outcome2を出力した時は、状態機械の出力としてoutcome4を出力し、状態機械の実行を終了します。
デバッグ出力の準備
# Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start()
ここでは、smach_viewerを用いて状態機械を確認するために、 状態機械の状態(ややこしいね…)を外部に出力する手続きをしています。
状態機械の実行
# Execute SMACH plan
outcome = sm.execute()
最後に、状態機械を実行します。
実行してみる
それでは、作成したサンプルプログラムを実行してみましょう。
rosrun smach_tutorial state_machine_simple.py
こんなのが出たら成功です。
smach_viewerを使う
rosrun smach_viewer smach_viewer.py
こんな感じで状態遷移図が表示されれば成功!