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看图形如下: