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

本文主要是介绍ROS smach-----创建层级状态机，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1 创建几个状态

# State Fooclass Foo(smach.State):def __init__(self, outcomes=['outcome1', 'outcome2'])def execute(self, userdata):return 'outcome1'# State Barclass Bar(smach.State):def __init__(self, outcomes=['outcome1'])def execute(self, userdata):return 'outcome4'# State Basclass Bas(smach.State):def __init__(self, outcomes=['outcome3'])def execute(self, userdata):return 'outcome3'

2 创建层级状态机

# Create the top level SMACH state machinesm_top = smach.StateMachine(outcomes=['outcome5'])# Open the containerwith 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 pythonimport 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 = 0def execute(self, userdata):rospy.loginfo('Executing state FOO')if self.counter < 3:self.counter += 1return '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 machinesm_top = smach.StateMachine(outcomes=['outcome5'])# Open the containerwith sm_top:smach.StateMachine.add('BAS', Bas(),transitions={'outcome3':'SUB'})# Create the sub SMACH state machinesm_sub = smach.StateMachine(outcomes=['outcome4'])# Open the containerwith sm_sub:# Add states to the containersmach.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 planoutcome = 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看图形如下：