executive_step_06.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     [INFO] : State machine starting in initial state 'DRAW_WITH_MONITOR' with userdata:
00019         []
00020     [INFO] : Concurrence starting with userdata:
00021         []
00022     [WARN] : Still waiting for action server 'turtle_shape1' to start... is it running?
00023     [WARN] : Still waiting for action server 'turtle_shape2' to start... is it running?
00024     [INFO] : Connected to action server 'turtle_shape2'.
00025     [INFO] : Connected to action server 'turtle_shape1'.
00026     [INFO] : Preempt requested on action 'turtle_shape2'
00027     [INFO] : Preempt on action 'turtle_shape2' cancelling goal:
00028     edges: 6
00029     radius: 0.5
00030     [INFO] : Concurrent Outcomes: {'MONITOR': 'invalid', 'DRAW': 'preempted'}
00031     [INFO] : State machine transitioning 'DRAW_WITH_MONITOR':'interrupted'-->'WAIT_FOR_CLEAR'
00032     [INFO] : State machine transitioning 'WAIT_FOR_CLEAR':'invalid'-->'DRAW_WITH_MONITOR'
00033     [INFO] : Concurrence starting with userdata:
00034         []
00035     [INFO] : Concurrent Outcomes: {'MONITOR': 'preempted', 'DRAW': 'succeeded'}
00036     [INFO] : State machine terminating 'DRAW_WITH_MONITOR':'succeeded':'succeeded'
00037     [INFO] : Concurrent Outcomes: {'SMALL': 'succeeded', 'BIG': 'succeeded'}
00038     [INFO] : State machine terminating 'DRAW_SHAPES':'succeeded':'succeeded'
00039 
00040 """
00041 
00042 import roslib; roslib.load_manifest('smach_tutorials')
00043 import rospy
00044 
00045 import threading
00046 from math import sqrt, pow
00047 
00048 import smach
00049 from smach import StateMachine, ServiceState, SimpleActionState, MonitorState, IntrospectionServer, Concurrence
00050 
00051 import std_srvs.srv
00052 import turtlesim.srv
00053 import turtlesim.msg
00054 import turtle_actionlib.msg
00055 
00056 
00057 def main():
00058     rospy.init_node('smach_usecase_step_06')
00059 
00060     # Construct static goals
00061     polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
00062     polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) 
00063 
00064     # Create a SMACH state machine
00065     sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
00066 
00067     # Open the container
00068     with sm0:
00069         # Reset turtlesim
00070         StateMachine.add('RESET',
00071                 ServiceState('reset', std_srvs.srv.Empty),
00072                 {'succeeded':'SPAWN'})
00073 
00074         # Create a second turtle
00075         StateMachine.add('SPAWN',
00076                 ServiceState('spawn', turtlesim.srv.Spawn,
00077                     request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
00078                 {'succeeded':'TELEPORT1'})
00079 
00080         # Teleport turtle 1
00081         StateMachine.add('TELEPORT1',
00082                 ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00083                     request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
00084                 {'succeeded':'TELEPORT2'})
00085 
00086         # Teleport turtle 2
00087         StateMachine.add('TELEPORT2',
00088                 ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00089                     request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
00090                 {'succeeded':'DRAW_SHAPES'})
00091 
00092         # Draw some polygons
00093         shapes_cc = Concurrence(
00094                 outcomes=['succeeded','aborted','preempted'],
00095                 default_outcome='aborted',
00096                 outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
00097         StateMachine.add('DRAW_SHAPES',shapes_cc)
00098         with shapes_cc:
00099             # Draw a large polygon with the first turtle
00100             Concurrence.add('BIG',
00101                     SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
00102                         goal = polygon_big))
00103             # Draw a small polygon with the second turtle
00104             draw_monitor_cc = Concurrence(
00105                     ['succeeded','aborted','preempted'],
00106                     'aborted',
00107                     child_termination_cb = lambda so: True,
00108                     outcome_map = {
00109                         'succeeded':{'DRAW':'succeeded'},
00110                         'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
00111                         'aborted':{'MONITOR':'invalid'}})
00112             Concurrence.add('SMALL',draw_monitor_cc)
00113             with draw_monitor_cc:
00114                 Concurrence.add('DRAW',
00115                         SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
00116                             goal = polygon_small))
00117 
00118                 def turtle_far_away(ud, msg):
00119                     """Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
00120                     if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
00121                         return True
00122                     return False
00123                 Concurrence.add('MONITOR',
00124                         MonitorState('/turtle1/pose',turtlesim.msg.Pose,
00125                             cond_cb = turtle_far_away))
00126 
00127     # Attach a SMACH introspection server
00128     sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
00129     sis.start()
00130 
00131     # Set preempt handler
00132     smach.set_preempt_handler(sm0)
00133 
00134     # Execute SMACH tree in a separate thread so that we can ctrl-c the script
00135     smach_thread = threading.Thread(target = sm0.execute)
00136     smach_thread.start()
00137 
00138     # Signal handler
00139     rospy.spin()
00140 
00141 if __name__ == '__main__':
00142     main()


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