executive_step_04.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_04.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'-->'BIG'
00016     [WARN] : Still waiting for action server 'turtle_shape1' to start... is it running?
00017     [INFO] : Connected to action server 'turtle_shape1'.
00018     [INFO] 1279655938.783058: State machine transitioning 'BIG':'succeeded'-->'SMALL'
00019     [INFO] 1279655975.025202: State machine terminating 'SMALL':'succeeded':'succeeded'
00020 
00021 """
00022 
00023 import roslib; roslib.load_manifest('smach_tutorials')
00024 import rospy
00025 
00026 import threading
00027 
00028 import smach
00029 from smach import StateMachine, ServiceState, SimpleActionState, IntrospectionServer
00030 
00031 import std_srvs.srv
00032 import turtlesim.srv
00033 import turtle_actionlib.msg
00034 
00035 
00036 def main():
00037     rospy.init_node('smach_usecase_step_04')
00038 
00039     # Construct static goals
00040     polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
00041     polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 
00042 
00043     # Create a SMACH state machine
00044     sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
00045 
00046     # Open the container
00047     with sm0:
00048         # Reset turtlesim
00049         StateMachine.add('RESET',
00050                 ServiceState('reset', std_srvs.srv.Empty),
00051                 {'succeeded':'SPAWN'})
00052 
00053         # Create a second turtle
00054         StateMachine.add('SPAWN',
00055                 ServiceState('spawn', turtlesim.srv.Spawn,
00056                     request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
00057                 {'succeeded':'TELEPORT1'})
00058 
00059         # Teleport turtle 1
00060         StateMachine.add('TELEPORT1',
00061                 ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00062                     request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
00063                 {'succeeded':'TELEPORT2'})
00064 
00065         # Teleport turtle 2
00066         StateMachine.add('TELEPORT2',
00067                 ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00068                     request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
00069                 {'succeeded':'BIG'})
00070 
00071         # Draw a large polygon with the first turtle
00072         StateMachine.add('BIG',
00073                 SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
00074                     goal = polygon_big),
00075                 {'succeeded':'SMALL'})
00076 
00077         # Draw a small polygon with the second turtle
00078         StateMachine.add('SMALL',
00079                 SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
00080                     goal = polygon_small))
00081 
00082     # Attach a SMACH introspection server
00083     sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
00084     sis.start()
00085 
00086     # Set preempt handler
00087     smach.set_preempt_handler(sm0)
00088 
00089     # Execute SMACH tree in a separate thread so that we can ctrl-c the script
00090     smach_thread = threading.Thread(target = sm0.execute)
00091     smach_thread.start()
00092 
00093     # Signal handler
00094     rospy.spin()
00095 
00096 if __name__ == '__main__':
00097     main()


asmach_tutorials
Author(s): Jonathan Bohren
autogenerated on Thu Apr 24 2014 15:35:31