【ROS2】ステートマシンSmachパッケージの取得と使い方 - その1

1. ソースコードの取得

公式は smach メタ関数パッケージの ROS2 バージョンを提供していません。DeepX によって移植されたパッケージを使用できます。

• https://github.com/DeepX-inc/executive_smach
• https://github.com/DeepX-inc/executive_smach_visualization

上記 2 つのパッケージをワークスペースにクローンし、rosdep +colcon build +source

2. 定期的な取得

• https://www.guyuehome.com/1069
• http://wiki.ros.org/smach/Tutorials/Getting
• https://blog.csdn.net/weixin_43455581/article/details/97136945

インターネット上のサンプル チュートリアルはすべて ROS1 ですが、ROS2 バージョンに変更してみましょう。

まず cd で src ディレクトリに移動し、新しい機能パッケージを作成します。

ros2 pkg create --build-type ament_python --dependencies rclpy smach smach_ros

import rclpy
from rclpy.node import Node
import smach
import smach_ros

class SmachTestNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.get_logger().info("启动 demo 节点")

# 定义状态 Foo
class Foo(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

    def execute(self, userdata):
        print('Executing state FOO')
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

# 定义状态 Bar
class Bar(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome2'])

    def execute(self, userdata):
        print('Executing state BAR')
        return 'outcome2'

# main
def main(args=None):
    rclpy.init(args=args) # 初始化 ros
    node = SmachTestNode("execute_smach_test")

    # 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('my_smach_introspection_server', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm.execute()

    # Wait for ctrl-c to stop the application
    rclpy.spin(node)
    sis.stop()

if __name__ == '__main__':
    main()

ノードを起動すると表示されます

別のターミナルを入力してros2 run smach_viewer smach_viewer_gui.pyステートマシンを視覚化します

3 ルーチン分析

ステートマシンには 4 つの概念があります。

• 状態、状態。ステート マシンには少なくとも 2 つの状態が含まれている必要があります。たとえば、自動ドアには開いた状態と閉じた状態の 2 つの状態があります。
• イベント、イベント。イベントは、操作を実行するためのトリガー条件またはパスワードです。自動ドアの場合は「ドア開ボタンを押す」がイベントとなります。
• アクション、アクション。イベント発生後に実行されるアクション。たとえば、イベントは「ボタンを押してドアを開ける」であり、アクションは「ドアを開ける」です。プログラミングする場合、通常、アクションは関数に対応します。
• 移行、変革。つまり、ある状態から別の状態に変化することです。例えば「ドアを開ける処理」は変形です。

ステート マシンとしては、まず状態を持つ必要があります。このルーチンには FOO と BAR の 2 つの状態があります。これら 2 つの状態は Python 関数によって定義され、初期化 ( init ) と実行 (execute)の 2 つの関数を含む構造は類似しています。

3.1 状態初期化関数

初期化関数は、状態クラスを初期化し、smac で状態の初期化関数を呼び出すために使用され、出力状態を定義する必要があります: 結果 1、結果 2

def __init__(self):
        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
        self.counter = 0

ここでの結果は、文字列で表される状態の終了時の出力値を表し、値の範囲はユーザーによって定義されます。たとえば、ステートの実行が成功したかどうかを定義できます: ['succeeded'、'failed'、'awesome'] 各ステートは複数の出力値を持つことができ、異なる出力に従って異なる次のステートにジャンプすることができます。価値観。

注: 初期化関数はブロックできません。同期などのブロック関数を実装する必要がある場合は、マルチスレッド実装を使用できます。

3.2 アクション実行機能

実行関数は、各ステートにおける特定の作業内容であり、ブロッキング作業を実行でき、作業が定義された出力値を返す必要がある場合、ステートは終了します。

def execute(self, userdata):
        print('Executing state FOO')
        if self.counter < 3:
            self.counter += 1
            return 'outcome1'
        else:
            return 'outcome2'

3.3 メイン関数

main関数では、まずROSノードを初期化します。

次に、StateMachine を使用してステート マシンを作成し、ステート マシンの実行後の 2 つの最終出力値、outcome4 とoutcome5 を指定します。

# 创建一个 SMACH 状态机
    sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])

SMACH ステート マシンはコンテナです。add メソッドを使用して必要なステートをステート マシン コンテナに追加でき、同時にステート間のジャンプ関係を設定する必要があります。

# 打开容器
    with sm:
    # 将状态添加到容器中
        smach.StateMachine.add('FOO', Foo(), 
            transitions={
    
    'outcome1':'BAR', 'outcome2':'outcome4'})
        smach.StateMachine.add('BAR', Bar(), 
            transitions={
    
    'outcome2':'FOO'})

たとえば、最初に「FOO」という名前の状態をステート マシンに追加します。この状態のクラスは前に定義した Foo で、遷移は状態の変化を表します(つまり、ステート マシンの 4 番目の概念)。ステートが実行され、outcome1 が出力され、その後「BAR」ステートにジャンプします。出力 outout2 が実行された場合は、このステート マシンが終了し、outcome4 が出力されます。

上で見たビジュアル インターフェイスを思い出してください。ステート マシンを視覚化するには、コードにビジュアル サーバーを追加する必要があります。

# Create and start the introspection server
  sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm, '/SM_ROOT')
  sis.start()

IntrospectionServer() メソッドは、次の 3 つのパラメータを持つ内部視覚化サーバーを作成するために使用されます。

1. 最初のパラメータはサーバーの名前で、必要に応じて自由に指定できます。
2. 2 番目のパラメータは監視するステート マシンです。
3. SMACH ステート マシンはネストをサポートしており、ステートは独自のステート マシンを持つこともできるため、3 番目のパラメーターはステート マシンのレベルを表します。

次に、execute() メソッドを使用してステート マシンの実行を開始できます。

# Execute SMACH plan
    outcome = sm.execute()

実行後、内部可視化サーバーを停止する必要があります。

sis.stop()

4. まとめ

次に、ステート マシン全体を確認してみましょう。

図からわかるように、ステート マシンが動作を開始すると、最初に追加した最初の状態 "FOO" にジャンプし、この状態でカウンタ変数を累積します。カウンタが 3 未満の場合、outcome1 を出力します。状態終了後にジャンプして「BAR」状態へ移行します。「BAR」状態では何も行われず、出力outcome2は「FOO」状態に戻ります。このように何度か行ったり来たりした後、カウンタは 3 になり、「FOO」ステートの出力値はoutcome2になり、その後outcome4にジャンプします。これは、有限ステートマシンが終了したことを意味します。Outcome5 はプロセス全体には関与していないため、グラフ上では孤立したノードになります。

上記のステート マシンは、単純なロボット アプリケーションとして想像できます。ロボットはテーブル上のカップをつかみ、掴めたらタスクを終了し、掴めなければ試行を続け、掴めなければ諦めます。 3 回試した後、掴んでタスクを終了します。

追記: 複雑なステートマシン: