ROS smach-----创建层级状态机

本文介绍如何使用ROS的SMACH库构建状态机,包括创建基本状态、层级状态机及其实例代码。通过示例展示了状态之间的转换过程及使用smach_viewer进行可视化。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

1 创建几个状态

# State Foo
  class Foo(smach.State):
     def __init__(self, outcomes=['outcome1', 'outcome2'])
     
     def execute(self, userdata):
        return 'outcome1'


  # State Bar
  class Bar(smach.State):
     def __init__(self, outcomes=['outcome1'])
     
     def execute(self, userdata):
        return 'outcome4'


  # State Bas
  class Bas(smach.State):
     def __init__(self, outcomes=['outcome3'])
     
     def execute(self, userdata):
        return 'outcome3'

2 创建层级状态机

# Create the top level SMACH state machine
    sm_top = smach.StateMachine(outcomes=['outcome5'])

    # Open the container
    with sm_top:

        smach.StateMachine.add('BAS', Bas(),
                               transitions={'outcome3':'SUB'})

        # Create the sub SMACH state machine 
        sm_sub = smach.StateMachine(outcomes=['outcome4'])

        # Open the container 
        with sm_sub:

            # Add states to the container 
            smach.StateMachine.add('FOO', Foo(),
                                   transitions={'outcome1':'BAR', 
                                                'outcome2':'outcome4'})
            smach.StateMachine.add('BAR', Bar(),
                                   transitions={'outcome1':'FOO'})

        smach.StateMachine.add('SUB', sm_sub,
                               transitions={'outcome4':'outcome5'})

3  实例代码

#!/usr/bin/env python

import roslib; roslib.load_manifest('smach_tutorials')
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')
        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'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAR')
        return 'outcome1'
        


# define state Bas
class Bas(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['outcome3'])

    def execute(self, userdata):
        rospy.loginfo('Executing state BAS')
        return 'outcome3'




def main():
    rospy.init_node('smach_example_state_machine')

    # Create the top level SMACH state machine
    sm_top = smach.StateMachine(outcomes=['outcome5'])
    
    # Open the container
    with sm_top:

        smach.StateMachine.add('BAS', Bas(),
                               transitions={'outcome3':'SUB'})

        # Create the sub SMACH state machine
        sm_sub = smach.StateMachine(outcomes=['outcome4'])

        # Open the container
        with sm_sub:

            # Add states to the container
            smach.StateMachine.add('FOO', Foo(), 
                                   transitions={'outcome1':'BAR', 
                                                'outcome2':'outcome4'})
            smach.StateMachine.add('BAR', Bar(), 
                                   transitions={'outcome1':'FOO'})

        smach.StateMachine.add('SUB', sm_sub,
                               transitions={'outcome4':'outcome5'})

    # Execute SMACH plan
    outcome = sm_top.execute()



if __name__ == '__main__':
    main()


运行结果

[INFO] [WallTime: 1478507071.756471] State machine starting in initial state 'BAS' with userdata: 
	[]
[INFO] [WallTime: 1478507071.756705] Executing state BAS
[INFO] [WallTime: 1478507071.756850] State machine transitioning 'BAS':'outcome3'-->'SUB'
[INFO] [WallTime: 1478507071.757021] State machine starting in initial state 'FOO' with userdata: 
	[]
[INFO] [WallTime: 1478507071.757197] Executing state FOO
[INFO] [WallTime: 1478507071.757327] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478507071.757453] Executing state BAR
[INFO] [WallTime: 1478507071.757573] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478507071.757696] Executing state FOO
[INFO] [WallTime: 1478507071.757816] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478507071.757937] Executing state BAR
[INFO] [WallTime: 1478507071.758055] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478507071.758175] Executing state FOO
[INFO] [WallTime: 1478507071.758291] State machine transitioning 'FOO':'outcome1'-->'BAR'
[INFO] [WallTime: 1478507071.758410] Executing state BAR
[INFO] [WallTime: 1478507071.758525] State machine transitioning 'BAR':'outcome1'-->'FOO'
[INFO] [WallTime: 1478507071.758645] Executing state FOO
[INFO] [WallTime: 1478507071.758779] State machine terminating 'FOO':'outcome2':'outcome4'
[INFO] [WallTime: 1478507071.758901] State machine terminating 'SUB':'outcome4':'outcome5'



如果用smach_viewer看图形如下:


评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值