うごくものづくりのために

技術的な備忘録がメインです。

状態機械を作る"Smach"パッケージの使い方(1)

ロボットに限らずですが、状態機械を用いることで、様々なタスクの遷移をスマートに表現することができます。

ROSの"Smach"パッケージを使えば、状態機械をPythonスクリプトによって比較的簡単に実現できます。

ja/smach - ROS Wiki

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()

やっていることは、

  1. 状態の作成
  2. 状態機械の作成
  3. デバッグ出力の準備
  4. 状態機械の実行

の順となります。

サンプルのコードを上から順に見ていきます。

状態の作成

# 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

f:id:tilt_silvie:20150524214119p:plain

こんなのが出たら成功です。

smach_viewerを使う

rosrun smach_viewer smach_viewer.py

f:id:tilt_silvie:20150524214306p:plain

こんな感じで状態遷移図が表示されれば成功!