state_machine_nesting.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """
00003 Description:
00004     Create one state machine, put a state in it that sets something in the
00005     userdata key 'x'. Put another state machine inside of that state machine,
00006     tell it to fetch the userdata key 'x'. Put a state that reads 'x' into
00007     that nested state machine, and spew some info to rosout.
00008 
00009 Usage:
00010     $> ./state_machine_nesting.py
00011 
00012 Output:
00013     [INFO] : State machine starting in initial state 'SET' with userdata: 
00014         []
00015     [INFO] : State machine transitioning 'SET':'set_it'-->'NESTED'
00016     [INFO] : State machine starting in initial state 'GET' with userdata: 
00017         ['x']
00018     [INFO] : >>> GOT DATA! x = hello
00019     [INFO] : State machine terminating 'GET':'got_it':'done'
00020     [INFO] : State machine terminating 'NESTED':'done':'succeeded'
00021 
00022 """
00023 
00024 import roslib; roslib.load_manifest('asmach_tutorials')
00025 import rospy
00026 import asmach as smach
00027 import smach_ros
00028 
00029 # Define a state to set some user data
00030 class Setter(smach.State):
00031     def __init__(self, val):
00032         smach.State.__init__(self, outcomes = ['set_it'], output_keys = ['x'])
00033         self._val = val
00034     def execute(self, ud):
00035         # Set the data
00036         ud.x = self._val
00037         rospy.loginfo('>>> Set data: %s' % str(self._val))
00038         return 'set_it'
00039 
00040 # Define a state to get some user data
00041 class Getter(smach.State):
00042     def __init__(self):
00043         smach.State.__init__(self, outcomes = ['got_it'], input_keys = ['x'])
00044     def execute(self, ud):
00045         # Wait for data to appear
00046         while 'x' not in ud:
00047             rospy.loginfo('>>> Waiting for data...')
00048             rospy.sleep(0.5)
00049         rospy.loginfo('>>> GOT DATA! x = '+str(ud.x))
00050         return 'got_it'
00051 
00052 def main():
00053     rospy.init_node('smach_example_state_machine_nesting')
00054 
00055     # Create a SMACH state machine
00056     sm0 = smach.StateMachine(outcomes=['succeeded'])
00057 
00058     # Open the container
00059     with sm0:
00060         # Add states to the container
00061         smach.StateMachine.add('SET', Setter(val='hello'), {'set_it':'NESTED'})
00062 
00063         sm1 = smach.StateMachine(outcomes=['done'], input_keys=['x'])
00064         smach.StateMachine.add('NESTED', sm1, {'done':'succeeded'})
00065         with sm1:
00066             smach.StateMachine.add('GET', Getter(), {'got_it':'done'})
00067 
00068     # Execute SMACH plan
00069     outcome = sm0.execute()
00070 
00071 if __name__ == '__main__':
00072     main()


asmach_tutorials
Author(s): Jonathan Bohren
autogenerated on Wed Sep 16 2015 04:38:38