ROS2演習14-2021：Smachで作るステートマシーン

2022.01.20

本記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今回はSmach（State Machine, スマッチと発音することが多いようです．）を使ったステートマシン（状態遷移機械）の作り方を説明します．Smachを使うと簡単に状態マシンをプログラムで書くことができます．

なお，この記事は参考文献のサンプルコードをROS2化したものです．説明は参考サイトを参考に意訳し，わかりやすいように説明を追加しています．

参考サイト

Smachのインストール

ROS2 Foxyに対応したパッケージを以下の手順でインストールします．このパッケージはSmach viewerには対応していないようです．

  • $ cd ~/airobot_ws/src
  • $ git clone https://github.com/jeffrey870517/executive_smach
  • $ cd ..
  • $ colcon build
  • $ source install/setup.bash

 

サンプル1：シンプルな２状態ステートマシン

まず，２つの状態を持つ簡単なステートマシンを考えます．状態はFOOとBARです．初期状態はFOOで状態が終わるとoutcome1を出力し状態BARへ遷移します．状態BARは状態が終わると状態FOOへ遷移してoutcome1を出力します．赤字のoutcome4はステートマシンの可能性のある出力です．outcome4は状態ではないので，ステートマシンの最終出力です．この状態遷移図をSmachを使ってROS2とPythonで書いたのが以下のコードです．以下のコードは参考サイトのサンプルプログラムはROS1用なのでそれをROS2化したものです．

# ファイル名：state_machine2.py
#!/usr/bin/env python
import rclpy
from rclpy.node import Node
import smach

rclpy.init()
_node = Node('state_machin2')


# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0
        self.logger = _node.get_logger()

    def execute(self, userdata):
        self.logger.info('状態FOOを実行中')
        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=['outcome1'])
        self.logger = _node.get_logger()

    def execute(self, userdata):
        self.logger.info('状態BARを実行中')
        return 'outcome1'


def main():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])

    # 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={'outcome1':'FOO'})

    # Execute SMACH plan
    outcome = sm.execute()


if __name__ == '__main__':
    main()
  • コンストラクタ__init__でステートマシンを初期化します（12行目）：この例のFOOでは，smach.Stateクラスのコンストラクタを呼び出しています．FOOは状態が終わると，’outcome1’と’outcome2’を出力するようここで設定しています．あとは，メンバ変数のcounterを0に初期化し，_node.get_logger()と毎回書くのは大変なので，self.loggerで済ませるようにしています．
  • executeメソッドで各状態が実行する処理を書きます（17行目）．この例では，”状態FOOを実行中”と出力し，こでは， countの値が3より小さかったらcountに１を足して’outcome1’を返し，3より大きかったら’outcome2’を返しています．次の状態に遷移します．
  • outcomeの説明：各状態は終了するとoutcomeを返します．outcomeは結果という意味です．outcomeは一つでも何個でも持つことができます．outcomeはユーザが定義する文字列で，どのように状態が終了するかを書いています．outcomeの例としては [‘succeeded’, ‘failed’, ‘awesome’]などが考えられます．次の状態への遷移は，前の状態のoutcomeによって指定されます．
  • ステートマシンの作成（39行目）：smach.StateMachineのインスタンスとしてステートマシンsmを作成します．この例では，smの出力は’outcome4’としています．なお，
  • 状態の追加（42～45行目）：ここでステートマシンsmに状態を追加します．smach.StateMachine.addが実施に追加するメソッドで，１番目に引数’FOO’は状態名，2番目は辞書型で，出力とその遷移先を書きます．なお，42行目のwithはPythonのwith文で，必要な処理をしてくれるものです．これにより，後処理などを実装しなくてもよいので簡単で安全なプログラムになります．
  • 実行（48行目）：execute()でステートマシンを実行します．

 

実行方法

  • $ cd ~/colcon_ws/src/executive_smach/smach_napp/smach_napp
  • 上のソースコードをコピペして，state_machine2.pyというファイルを作成して保存する．
    • $ gedit state_machine2.py
  • $ source ~/colcon_ws/install/setup.bash
  • $ python3 state_machine2.py

 

練習：シンプルな３状態ステートマシン

以下のコードを読んで，サンプル１のようなSmachの状態遷移図を書いてみましょう．なお，このコードは参考サイトのサンプルプログラムをROS2化したものです．

#!/usr/bin/env python
import rclpy
from rclpy.node import Node
import smach
import smach_ros

class ExampleState(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes = ['done'])
        # Create node
        #self.node = Node("example_state")

    def execute(self, ud):
        return 'done'


def main():
    rclpy.init()
    node = Node("state_machine")

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted'])

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', ExampleState(), {'done':'BAR'})
        smach.StateMachine.add('BAR', ExampleState(), {'done':'BAZ'})
        smach.StateMachine.add('BAZ', ExampleState(), {'done':'succeeded'})

    # Execute SMACH plan
    outcome = sm.execute()


if __name__ == '__main__':
    main()

サンプル２：ユーザデータの受け渡し
サンプル１では状態遷移だけをするステートマシンでした．サンプル２では，各状態間でデータをやり取りする方法を説明します．

  • userdata：ユーザデータは状態に入出力されるデータの構造体です．
  • input_keys： その状態が実行するために必要な全入力リストで，userdataの属性（field）に入ります．executeメソッドは，ユーザデータ構造体のコピーが渡されます．状態は全てのuserdataの属性を読むことができますが，書くことはできません．
  • output_keys：その状態が出力する全出力リストで，状態はユーザデータ構造体のすべての属性に書き込むことができます．
  • remapping：リマッピングは，状態の入出力とステートマシンの入出力を対応づけます．

以下のコードは参考サイトのサンプルプログラムをROS2化したものです．

#!/usr/bin/env python
import rclpy
from rclpy.node import Node
import smach

# create node
rclpy.init()
_node = Node('smach_example_state_machine')

# define state Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self,
                             outcomes=['outcome1','outcome2'],
                             input_keys=['foo_counter_in'],
                             output_keys=['foo_counter_out'])
        self.logger = _node.get_logger()

    def execute(self, userdata):
        self.logger.info('Executing state FOO')
        if userdata.foo_counter_in < 3:
            userdata.foo_counter_out = userdata.foo_counter_in + 1
            return 'outcome1'
        else:
            return 'outcome2'


# define state Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self,
                             outcomes=['outcome1'],
                             input_keys=['bar_counter_in'])
        self.logger = _node.get_logger()

    def execute(self, userdata):
        self.logger.info('Executing state BAR')
        self.logger.info('Counter = %f'%userdata.bar_counter_in)
        return 'outcome1'


def main():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['outcome4'])
    sm.userdata.sm_counter = 0

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('FOO', Foo(),
                               transitions={'outcome1':'BAR',
                                            'outcome2':'outcome4'},
                               remapping={'foo_counter_in':'sm_counter',
                                          'foo_counter_out':'sm_counter'})
        smach.StateMachine.add('BAR', Bar(),
                               transitions={'outcome1':'FOO'},
                               remapping={'bar_counter_in':'sm_counter'})


    # Execute SMACH plan
    outcome = sm.execute()

演習問題

  • 授業で説明した自動販売機のステートマシンをSmachで実装してください．

終わり

コメント

タイトルとURLをコピーしました