executive_step_05.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 """
00003 Description:
00004 
00005 Usage:
00006     $> roslaunch turtle_nodes.launch
00007     $> ./executive_step_06.py
00008 
00009 Output:
00010     [INFO] : State machine starting in initial state 'RESET' with userdata:
00011             []
00012     [INFO] : State machine transitioning 'RESET':'succeeded'-->'SPAWN'
00013     [INFO] : State machine transitioning 'SPAWN':'succeeded'-->'TELEPORT1'
00014     [INFO] : State machine transitioning 'TELEPORT1':'succeeded'-->'TELEPORT2'
00015     [INFO] : State machine transitioning 'TELEPORT2':'succeeded'-->'DRAW_SHAPES'
00016     [INFO] : Concurrence starting with userdata: 
00017             []
00018     [WARN] : Still waiting for action server 'turtle_shape2' to start... is it running?
00019     [WARN] : Still waiting for action server 'turtle_shape1' to start... is it running?
00020     [INFO] : Connected to action server 'turtle_shape2'.
00021     [INFO] : Connected to action server 'turtle_shape1'.
00022     [INFO] : Concurrent Outcomes: {'SMALL': 'succeeded', 'BIG': 'succeeded'}
00023     [INFO] : State machine terminating 'DRAW_SHAPES':'succeeded':'succeeded'
00024 """
00025 
00026 import roslib; roslib.load_manifest('smach_tutorials')
00027 import rospy
00028 
00029 import threading
00030 
00031 import smach
00032 from smach import StateMachine, ServiceState, SimpleActionState, IntrospectionServer, Concurrence
00033 
00034 import std_srvs.srv
00035 import turtlesim.srv
00036 import turtle_actionlib.msg
00037 
00038 
00039 def main():
00040     rospy.init_node('smach_usecase_step_05')
00041 
00042     # Construct static goals
00043     polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
00044     polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 
00045 
00046     # Create a SMACH state machine
00047     sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
00048 
00049     # Open the container
00050     with sm0:
00051         # Reset turtlesim
00052         StateMachine.add('RESET',
00053                 ServiceState('reset', std_srvs.srv.Empty),
00054                 {'succeeded':'SPAWN'})
00055 
00056         # Create a second turtle
00057         StateMachine.add('SPAWN',
00058                 ServiceState('spawn', turtlesim.srv.Spawn,
00059                     request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
00060                 {'succeeded':'TELEPORT1'})
00061 
00062         # Teleport turtle 1
00063         StateMachine.add('TELEPORT1',
00064                 ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00065                     request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
00066                 {'succeeded':'TELEPORT2'})
00067 
00068         # Teleport turtle 2
00069         StateMachine.add('TELEPORT2',
00070                 ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00071                     request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
00072                 {'succeeded':'DRAW_SHAPES'})
00073 
00074         # Draw some polygons
00075         shapes_cc = Concurrence(
00076                 outcomes=['succeeded','aborted','preempted'],
00077                 default_outcome='aborted',
00078                 outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
00079         StateMachine.add('DRAW_SHAPES',shapes_cc)
00080         with shapes_cc:
00081             # Draw a large polygon with the first turtle
00082             Concurrence.add('BIG',
00083                     SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
00084                         goal = polygon_big))
00085 
00086             # Draw a small polygon with the second turtle
00087             Concurrence.add('SMALL',
00088                     SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
00089                         goal = polygon_small))
00090 
00091 
00092     # Attach a SMACH introspection server
00093     sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
00094     sis.start()
00095 
00096     # Set preempt handler
00097     smach.set_preempt_handler(sm0)
00098 
00099     # Execute SMACH tree in a separate thread so that we can ctrl-c the script
00100     smach_thread = threading.Thread(target = sm0.execute)
00101     smach_thread.start()
00102 
00103     # Signal handler
00104     rospy.spin()
00105 
00106 if __name__ == '__main__':
00107     main()


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