state_machine_nesting2.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('asmach_tutorials')
00004 import rospy
00005 import asmach as smach
00006 import smach_ros
00007 
00008 # define state Foo
00009 class Foo(smach.State):
00010     def __init__(self):
00011         smach.State.__init__(self, outcomes=['outcome1','outcome2'])
00012         self.counter = 0
00013 
00014     def execute(self, userdata):
00015         rospy.loginfo('Executing state FOO')
00016         if self.counter < 3:
00017             self.counter += 1
00018             return 'outcome1'
00019         else:
00020             return 'outcome2'
00021 
00022 
00023 # define state Bar
00024 class Bar(smach.State):
00025     def __init__(self):
00026         smach.State.__init__(self, outcomes=['outcome1'])
00027 
00028     def execute(self, userdata):
00029         rospy.loginfo('Executing state BAR')
00030         return 'outcome1'
00031         
00032 
00033 
00034 # define state Bas
00035 class Bas(smach.State):
00036     def __init__(self):
00037         smach.State.__init__(self, outcomes=['outcome3'])
00038 
00039     def execute(self, userdata):
00040         rospy.loginfo('Executing state BAS')
00041         return 'outcome3'
00042 
00043 
00044 
00045 
00046 def main():
00047     rospy.init_node('smach_example_state_machine')
00048 
00049     # Create the top level SMACH state machine
00050     sm_top = smach.StateMachine(outcomes=['outcome5'])
00051     
00052     # Open the container
00053     with sm_top:
00054 
00055         smach.StateMachine.add('BAS', Bas(),
00056                                transitions={'outcome3':'SUB'})
00057 
00058         # Create the sub SMACH state machine
00059         sm_sub = smach.StateMachine(outcomes=['outcome4'])
00060 
00061         # Open the container
00062         with sm_sub:
00063 
00064             # Add states to the container
00065             smach.StateMachine.add('FOO', Foo(), 
00066                                    transitions={'outcome1':'BAR', 
00067                                                 'outcome2':'outcome4'})
00068             smach.StateMachine.add('BAR', Bar(), 
00069                                    transitions={'outcome1':'FOO'})
00070 
00071         smach.StateMachine.add('SUB', sm_sub,
00072                                transitions={'outcome4':'outcome5'})
00073 
00074     # Execute SMACH plan
00075     outcome = sm_top.execute()
00076 
00077 
00078 
00079 if __name__ == '__main__':
00080     main()


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