executive_step_07.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':'DRAW_SHAPES'})
00085 
00086         # Draw some polygons
00087         shapes_cc = Concurrence(
00088                 outcomes=['succeeded','aborted','preempted'],
00089                 default_outcome='aborted',
00090                 outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}})
00091         StateMachine.add('DRAW_SHAPES',shapes_cc)
00092         with shapes_cc:
00093             # Draw a large polygon with the first turtle
00094             Concurrence.add('BIG',
00095                     SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
00096                         goal = polygon_big))
00097 
00098             # Draw a small polygon with the second turtle
00099             small_shape_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
00100             Concurrence.add('SMALL',small_shape_sm)
00101             with small_shape_sm:
00102                 # Teleport turtle 2
00103                 StateMachine.add('TELEPORT2',
00104                         ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
00105                             request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
00106                         {'succeeded':'DRAW_WITH_MONITOR'})
00107 
00108                 # Construct a concurrence for the shape action and the monitor
00109                 draw_monitor_cc = Concurrence(
00110                         ['succeeded','aborted','preempted','interrupted'],
00111                         'aborted',
00112                         child_termination_cb = lambda so: True,
00113                         outcome_map = {
00114                             'succeeded':{'DRAW':'succeeded'},
00115                             'preempted':{'DRAW':'preempted','MONITOR':'preempted'},
00116                             'interrupted':{'MONITOR':'invalid'}})
00117 
00118                 StateMachine.add('DRAW_WITH_MONITOR',
00119                         draw_monitor_cc,
00120                         {'interrupted':'WAIT_FOR_CLEAR'})
00121 
00122                 with draw_monitor_cc:
00123                     Concurrence.add('DRAW',
00124                             SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
00125                                 goal = polygon_small))
00126                     def turtle_far_away(ud, msg):
00127                         """Returns True while turtle pose in msg is at least 1 unit away from (9,5)"""
00128                         if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0:
00129                             return True
00130                         return False
00131                     Concurrence.add('MONITOR',
00132                             MonitorState('/turtle1/pose',turtlesim.msg.Pose,
00133                                 cond_cb = turtle_far_away))
00134 
00135                 StateMachine.add('WAIT_FOR_CLEAR',
00136                         MonitorState('/turtle1/pose',turtlesim.msg.Pose,
00137                             cond_cb = lambda ud,msg: not turtle_far_away(ud,msg)),
00138                         {'valid':'WAIT_FOR_CLEAR','invalid':'TELEPORT2'})
00139 
00140 
00141     # Attach a SMACH introspection server
00142     sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
00143     sis.start()
00144 
00145     # Set preempt handler
00146     smach.set_preempt_handler(sm0)
00147 
00148     # Execute SMACH tree in a separate thread so that we can ctrl-c the script
00149     smach_thread = threading.Thread(target = sm0.execute)
00150     smach_thread.start()
00151 
00152     # Signal handler
00153     rospy.spin()
00154 
00155 if __name__ == '__main__':
00156     main()


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