def main(): rospy.init_node('exploration_fsm') sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: StateMachine.add('INITIAL_TURN', InitialTurnState(), transitions={ 'succeeded': 'EXPLORATION', 'aborted': 'EXPLORATION', 'preempted': 'preempted' }) StateMachine.add('EXPLORATION', simpleExplorationContainer(), transitions={'preempted': 'preempted'}) sis = smach_ros.IntrospectionServer('fsm_introspection', sm, '/EXPLORATION_FSM') sis.start() smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm.execute) smach_thread.start() rospy.spin() sis.stop()
def main(): rospy.init_node("fsm_dummy") sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with sm: StateMachine.add('INITIAL_TURN', SimpleActionState('/initial_turn', InitialTurnAction), transitions={ 'succeeded': 'EXPLORATION', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('EXPLORATION', exploration.ExplorationContainer(), transitions={ 'victim_thermal': 'IDENTIFICATION_SIMPLE', 'victim_camera': 'IDENTIFICATION_TRACKING', 'aborted': 'aborted', 'preempted': 'preempted', 'time_out': 'AGGRESSIVE_EXPLORATION' }) StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(), transitions={ 'parked': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(), transitions={ 'identification_finished': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' }) sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM') sis.start() smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm.execute) smach_thread.start() rospy.spin() sis.stop()
def main(): rospy.init_node("fsm_dummy") sm = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm: StateMachine.add('INITIAL_TURN', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={'succeeded':'EXPLORATION','aborted':'aborted','preempted':'preempted'}) StateMachine.add('EXPLORATION', exploration.ExplorationContainer(), transitions={'victim_thermal':'IDENTIFICATION_SIMPLE', 'victim_camera':'IDENTIFICATION_TRACKING','aborted':'aborted','preempted':'preempted','time_out':'AGGRESSIVE_EXPLORATION'}) StateMachine.add('IDENTIFICATION_SIMPLE', identification.IdentificationSimpleContainer(), transitions={'parked':'succeeded','aborted':'aborted','preempted':'preempted'}) StateMachine.add('IDENTIFICATION_TRACKING', identification.IdentificationTrackingContainer(), transitions={'identification_finished':'succeeded','aborted':'aborted','preempted':'preempted'}) StateMachine.add('AGGRESSIVE_EXPLORATION', SimpleActionState('/navigation/initial_turn', InitialTurnAction), transitions={'succeeded':'succeeded','aborted':'aborted','preempted':'preempted'}) sis = smach_ros.IntrospectionServer('smach_server', sm, '/DUMMY_FSM') sis.start() smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target = sm.execute) smach_thread.start() rospy.spin() sis.stop()
def main(): rospy.init_node('smach_usecase_step_04') # Construct static goals polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0) polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded','aborted','preempted']) # Open the container with sm0: # Reset turtlesim StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty), {'succeeded':'SPAWN'}) # Create a second turtle StateMachine.add('SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')), {'succeeded':'TELEPORT1'}) # Teleport turtle 1 StateMachine.add('TELEPORT1', ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)), {'succeeded':'TELEPORT2'}) # Teleport turtle 2 StateMachine.add('TELEPORT2', ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)), {'succeeded':'BIG'}) # Draw a large polygon with the first turtle StateMachine.add('BIG', SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction, goal = polygon_big), {'succeeded':'SMALL'}) # Draw a small polygon with the second turtle StateMachine.add('SMALL', SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction, goal = polygon_small)) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target = sm0.execute) smach_thread.start() # Signal handler rospy.spin()
def __init__(self, input_keys=[], output_keys=[]): outcomes = ['succeeded', 'aborted', 'preempted'] smach.Container.__init__(self, outcomes, input_keys, output_keys) self._root = None self._initial_task = None self._current_exec_root = None self._lock = threading.Lock() self._condition = threading.Condition(self._lock) self._result = None self._task_for_label = {} self._label_for_task = {} # Intentionally public self.userdata = smach.UserData() # Ensures that we halt when task is shut down smach_ros.set_preempt_handler(self)
def __init__(self, input_keys=[], output_keys=[]): outcomes = ["succeeded", "aborted", "preempted"] smach.Container.__init__(self, outcomes, input_keys, output_keys) self._root = None self._initial_task = None self._current_exec_root = None self._lock = threading.Lock() self._condition = threading.Condition(self._lock) self._result = None self._task_for_label = {} self._label_for_task = {} # Intentionally public self.userdata = smach.UserData() # Ensures that we halt when task is shut down smach_ros.set_preempt_handler(self)
def main(): rospy.init_node('smach_usecase_step_06') # Construct static goals polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0) polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded','aborted','preempted']) # Open the container with sm0: # Reset turtlesim StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty), {'succeeded':'SPAWN'}) # Create a second turtle StateMachine.add('SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')), {'succeeded':'TELEPORT1'}) # Teleport turtle 1 StateMachine.add('TELEPORT1', ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)), {'succeeded':'TELEPORT2'}) # Teleport turtle 2 StateMachine.add('TELEPORT2', ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)), {'succeeded':'DRAW_SHAPES'}) # Draw some polygons shapes_cc = Concurrence( outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}}) StateMachine.add('DRAW_SHAPES',shapes_cc) with shapes_cc: # Draw a large polygon with the first turtle Concurrence.add('BIG', SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction, goal = polygon_big)) # Draw a small polygon with the second turtle draw_monitor_cc = Concurrence( ['succeeded','aborted','preempted'], 'aborted', child_termination_cb = lambda so: True, outcome_map = { 'succeeded':{'DRAW':'succeeded'}, 'preempted':{'DRAW':'preempted','MONITOR':'preempted'}, 'aborted':{'MONITOR':'invalid'}}) Concurrence.add('SMALL',draw_monitor_cc) with draw_monitor_cc: Concurrence.add('DRAW', SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction, goal = polygon_small)) def turtle_far_away(ud, msg): """Returns True while turtle pose in msg is at least 1 unit away from (9,5)""" if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0: return True return False Concurrence.add('MONITOR', MonitorState('/turtle1/pose',turtlesim.msg.Pose, cond_cb = turtle_far_away)) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target = sm0.execute) smach_thread.start() # Signal handler rospy.spin()
def main(): rospy.init_node('fsm') sm = StateMachine(outcomes=['preempted']) with sm: StateMachine.add('MONITOR_START', MonitorModeState(robotModeMsg.MODE_START_AUTONOMOUS), transitions={ 'invalid': 'MONITOR_START', 'valid': 'ALL', 'preempted': 'preempted' }) sm_everything = StateMachine(outcomes=['preempted']) with sm_everything: StateMachine.add('WAIT_FOR_SLAM', Timer(10), transitions={ 'time_out': 'INITIAL_TURN', 'preempted': 'preempted' }) StateMachine.add('INITIAL_TURN', InitialTurnState(), transitions={ 'succeeded': 'ROBOT_MODE_EXPLORATION', 'aborted': 'ROBOT_MODE_EXPLORATION', 'preempted': 'preempted' }) StateMachine.add('ROBOT_MODE_EXPLORATION', ChangeRobotModeState( robotModeMsg.MODE_EXPLORATION), transitions={ 'succeeded': 'EXPLORATION', 'preempted': 'preempted' }) StateMachine.add('EXPLORATION', explorationWithVictims(), transitions={ 'camera_alert': 'MONITOR_VICTIM_AND_DO_WORK', 'thermal_alert': 'MONITOR_VICTIM_AND_DO_WORK', 'preempted': 'preempted' }) #~ sm.userdata.victim_info = None sm_victim = StateMachine( outcomes=['verified', 'not_verified', 'preempted', 'aborted'], output_keys=['victim_info']) with sm_victim: sm_victim.userdata.victim_info = None StateMachine.add('VICTIM_APPROACH', cameraIdentificationSimple(), transitions={ 'victim_approached': 'ROBOT_MODE_DF_HOLD', 'aborted': 'aborted', 'preempted': 'preempted' }) StateMachine.add('ROBOT_MODE_DF_HOLD', ChangeRobotModeState( robotModeMsg.MODE_DF_HOLD), transitions={ 'succeeded': 'DATA_FUSION_HOLD', 'preempted': 'preempted' }) StateMachine.add('DATA_FUSION_HOLD', dataFusionHold(), transitions={ 'verified': 'verified', 'not_verified': 'not_verified', 'preempted': 'preempted' }, remapping={'victim_info': 'victim_info'}) cc = Concurrence(outcomes=[ 'verified', 'not_verified', 'update_victim', 'preempted', 'aborted' ], default_outcome='not_verified', output_keys=['victim_info'], outcome_map={ 'verified': { 'VICTIM_IDENTIFICATION': 'verified', 'VICTIM_UPDATE': 'preempted' }, 'not_verified': { 'VICTIM_IDENTIFICATION': 'not_verified', 'VICTIM_UPDATE': 'preempted' }, 'preempted': { 'VICTIM_IDENTIFICATION': 'preempted', 'VICTIM_UPDATE': 'preempted' }, 'aborted': { 'VICTIM_IDENTIFICATION': 'aborted', 'VICTIM_UPDATE': 'preempted' }, 'update_victim': { 'VICTIM_UPDATE': 'update_victim', 'VICTIM_IDENTIFICATION': 'preempted' } }, child_termination_cb=_termination_cb) with cc: Concurrence.add('VICTIM_IDENTIFICATION', sm_victim, remapping={'victim_info': 'victim_info'}) Concurrence.add('VICTIM_UPDATE', MonitorVictimUpdateState()) StateMachine.add('MONITOR_VICTIM_AND_DO_WORK', cc, transitions={ 'verified': 'VICTIM_VALIDATION', 'not_verified': 'MONITOR_VICTIM_AND_DO_WORK', 'update_victim': 'MONITOR_VICTIM_AND_DO_WORK', 'preempted': 'preempted', 'aborted': 'ROBOT_MODE_EXPLORATION' }, remapping={'victim_info': 'victim_info'}) StateMachine.add('VICTIM_VALIDATION', validateVictim(), transitions={ 'valid': 'MONITOR_VICTIM_AND_DO_WORK', 'not_valid': 'MONITOR_VICTIM_AND_DO_WORK', 'preempted': 'preempted' }, remapping={'victim_info': 'victim_info'}) cc = Concurrence(outcomes=['preempted', 'shutdown'], default_outcome='preempted', outcome_map={ 'preempted': { 'AUTONOMOUS': 'preempted', 'MONITOR_SHUTDOWN': 'preempted' }, 'shutdown': { 'MONITOR_SHUTDOWN': 'valid', 'AUTONOMOUS': 'preempted' } }, child_termination_cb=_termination_cb_all) with cc: Concurrence.add('AUTONOMOUS', sm_everything) Concurrence.add( 'MONITOR_SHUTDOWN', MonitorModeState(robotModeMsg.MODE_TELEOPERATED_LOCOMOTION)) StateMachine.add('ALL', cc, transitions={ 'shutdown': 'MONITOR_START', 'preempted': 'preempted' }) sis = smach_ros.IntrospectionServer('fsm_introspection', sm, '/PANDORA_FSM') sis.start() smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm.execute) smach_thread.start() rospy.spin() sis.stop()
def polygonial(): ids = [2, 5, 1] #define the differents points test_point = Point() test_point.x = 0 test_point.y = 0 test_point.z = 0.5 home_points_fo8 = [Point(), Point(), Point()] home_points_fo8[0].x = 0.0 home_points_fo8[0].y = 0.0 home_points_fo8[0].z = 0.3 home_points_fo8[1].x = 0.3 home_points_fo8[1].y = 0.0 home_points_fo8[1].z = 0.3 home_points_fo8[2].x = -0.3 home_points_fo8[2].y = 0.0 home_points_fo8[2].z = 0.3 home_points_heli = [Point(), Point(), Point()] home_points_heli[0].x = 0.0 home_points_heli[0].y = 0.0 home_points_heli[0].z = 0.3 home_points_heli[1].x = 0.0 home_points_heli[1].y = 0.9 - 0.4 home_points_heli[1].z = 0.9 home_points_heli[2].x = -0.3 home_points_heli[2].y = 0.0 home_points_heli[2].z = 0.3 my_points = [ Point(), Point(), Point(), Point(), Point(), Point(), Point(), Point() ] my_points[0].x = 0.5 my_points[0].y = -0.5 my_points[0].z = 0.5 my_points[1].x = 0.7 my_points[1].y = 0.0 my_points[1].z = 0.5 my_points[2].x = 0.5 my_points[2].y = 0.5 my_points[2].z = 0.5 my_points[3].x = 0 my_points[3].y = 0.7 my_points[3].z = 0.5 my_points[4].x = -0.5 my_points[4].y = 0.5 my_points[4].z = 0.5 my_points[5].x = -0.7 my_points[5].y = 0 my_points[5].z = 0.5 my_points[6].x = -0.5 my_points[6].y = -0.5 my_points[6].z = 0.5 my_points[7].x = 0 my_points[7].y = -0.7 my_points[7].z = 0.5 # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) #progressively add drones with sm0: """ StateMachine.add('HELI_START', SimpleActionState('drone1detect_perimeter', my_newAction, goal = my_newGoal(point = home_points_heli[0], id = ids[0] )), transitions={'succeeded' : 'HELI_EXECUTE', 'aborted' : 'land_all', 'preempted' : 'land_all'}) StateMachine.add('HELI_EXECUTE', SimpleActionState('trajectory_action', doTrajAction, goal = doTrajGoal(shape = 1, id = ids[0] )), transitions={'succeeded' : 'HELI_END', 'aborted' : 'land_all', 'preempted' : 'land_all'}) StateMachine.add('HELI_END', SimpleActionState('land_drone1', my_newAction, goal = my_newGoal(point = my_points[3], id = ids[0]) ), transitions={'succeeded' : 'drone2-FIG8', 'aborted' : 'land_all', 'preempted' : 'land_all'}) """ StateMachine.add('drone2-FIG8', SimpleActionState('drone2detect_perimeter', my_newAction, goal=my_newGoal( point=home_points_fo8[2], id=ids[1])), transitions={ 'succeeded': 'drone3-FIG8', 'aborted': 'land_all', 'preempted': 'land_all' }) StateMachine.add('drone3-FIG8', SimpleActionState('drone3detect_perimeter', my_newAction, goal=my_newGoal( point=home_points_fo8[1], id=ids[2])), transitions={ 'succeeded': 'FIG8_EXECUTE', 'aborted': 'land_all', 'preempted': 'land_all' }) # fig8_sm = Concurrence(['drone2-2', 'land_all'], 'land_all', # outcome_map={'drone2-2' : {'FIG8_EXECUTE_drone3' : 'succeeded', 'FIG8_EXECUTE_drone2' : 'succeeded'}}) fig8_sm = Concurrence(['succeeded', 'aborted', 'preempted'], 'succeeded') #, #outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' }) StateMachine.add('FIG8_EXECUTE', fig8_sm, transitions={ 'succeeded': 'drone2-2', 'aborted': 'land_all', 'preempted': 'land_all' }) ## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!! with fig8_sm: Concurrence.add( 'FIG8_EXECUTE_drone2', SimpleActionState('fig8_drone2', doTrajAction, goal=doTrajGoal(shape=8, id=ids[1]))) Concurrence.add( 'FIG8_EXECUTE_drone3', SimpleActionState('fig8_drone3', doTrajAction, goal=doTrajGoal(shape=8, id=ids[2]))) heli_sm = Concurrence(['succeeded', 'aborted', 'preempted'], 'succeeded') #, #outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' }) StateMachine.add('HELI_EXECUTE', heli_sm, transitions={ 'succeeded': 'drone2-2', 'aborted': 'land_all', 'preempted': 'land_all' }) ## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!! with heli_sm: Concurrence.add( 'HELI_EXECUTE_drone2', SimpleActionState('heli_drone2', doTrajAction, goal=doTrajGoal(shape=0, id=ids[1]))) Concurrence.add( 'HELI_EXECUTE_drone3', SimpleActionState('heli_drone3', doTrajAction, goal=doTrajGoal(shape=0, id=ids[2]))) fig8_end = Concurrence(['succeeded', 'aborted', 'preempted'], 'succeeded') #, #outcome_map={'succeeded' : 'drone2-2', 'preempted' : 'land_all', 'aborted' : 'land_all' }) StateMachine.add('FIG8_END', fig8_end, transitions={ 'succeeded': 'drone2-2', 'aborted': 'land_all', 'preempted': 'land_all' }) ## Bug BBL: DO NOT try to decide on outcomes in a concurrence, do it in the SM.add!! with fig8_end: Concurrence.add( 'FIG8_END_drone2', SimpleActionState('land_drone2', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[1]))) Concurrence.add( 'FIG8_END_drone3', SimpleActionState('land_drone3', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[2]))) StateMachine.add('drone2-2', SimpleActionState('drone2detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[2], id=ids[1])), transitions={ 'succeeded': 'drone3-1', 'aborted': 'land_all', 'preempted': 'land_all' }) StateMachine.add('drone3-1', SimpleActionState('drone3detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[0], id=ids[2])), transitions={ 'succeeded': 'drone1-3', 'aborted': 'land_all', 'preempted': 'land_all' }) StateMachine.add('drone1-3', SimpleActionState('drone1detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[4], id=ids[0])), transitions={ 'succeeded': 'infinit_loop', 'aborted': 'land_all', 'preempted': 'land_all' }) land_sm = Concurrence(['succeeded', 'aborted', 'preempted'], 'succeeded') StateMachine.add('land_all', land_sm) with land_sm: # DUPLICATA FOR HIGH LEVEL # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE1', SimpleActionState('land_drone1', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[0]))) # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE2', SimpleActionState('land_drone2', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[1]))) # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE3', SimpleActionState('land_drone3', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[2]))) ############################################ sm1 = Concurrence( ['succeeded', 'aborted', 'preempted'], 'succeeded', #child_termination_cb = lambda so: True, #outcome_map = { #'succeeded':{'WAIT_FOR_CLEAR':'valid'}, #'aborted':{'DRONE1':'aborted'}} ) StateMachine.add('infinit_loop', sm1) with sm1: drone1 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] test_drone = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] #Concurrence.add('DRONE1', test_drone) with test_drone: order = (5, 6, 7, 0, 1, 2, 3, 4) for i in range(7): point_for_state = Point() point_for_state.x = test_point.x point_for_state.y = test_point.y if i <= 4: point_for_state.z = test_point.z + (i * 0.1) else: point_for_state.z = test_point.z - (i * 0.1) + 0.8 StateMachine.add('DRONE1-' + str(order[i]), SimpleActionState( 'drone1detect_perimeter', my_newAction, goal=my_newGoal(point=point_for_state, id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[i + 1]), 'aborted': 'LAND_DRONE1', 'preempted': 'LAND_DRONE1' }) #make it infinit smach.StateMachine.add( 'DRONE1-' + str(order[-1]), SimpleActionState('drone1detect_perimeter', my_newAction, goal=my_newGoal(point=test_point, id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[0]), 'aborted': 'LAND_DRONE1', 'preempted': 'LAND_DRONE1' }) Concurrence.add('DRONE1', drone1) # Open the container with drone1: #add each state order = (5, 6, 7, 0, 1, 2, 3, 4) for i in range(7): point_for_state = Point() point_for_state.x = my_points[order[i]].x point_for_state.y = my_points[order[i]].y if i <= 4: point_for_state.z = my_points[order[i]].z + (i * 0.1) else: point_for_state.z = my_points[order[i]].z - (i * 0.1) + 0.8 StateMachine.add('DRONE1-' + str(order[i]), SimpleActionState( 'drone1detect_perimeter', my_newAction, goal=my_newGoal(point=point_for_state, id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[i + 1]), 'aborted': 'LAND_DRONE1', 'preempted': 'LAND_DRONE1' }) #make it infinit smach.StateMachine.add( 'DRONE1-' + str(order[-1]), SimpleActionState('drone1detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[-1]], id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[0]), 'aborted': 'LAND_DRONE1', 'preempted': 'LAND_DRONE1' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE1', SimpleActionState('land_drone1', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[0])), transitions={'succeeded': 'LAND_DRONE1'}) drone2 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] Concurrence.add('DRONE2', drone2) # Open the container with drone2: #add each state order = (3, 4, 5, 6, 7, 0, 1, 2) for i in range(7): point_for_state = Point() point_for_state.x = my_points[order[i]].x point_for_state.y = my_points[order[i]].y if i <= 4: point_for_state.z = my_points[order[i]].z + (i * 0.1) else: point_for_state.z = my_points[order[i]].z - (i * 0.1) + 0.8 StateMachine.add('DRONE2-' + str(order[i]), SimpleActionState( 'drone2detect_perimeter', my_newAction, goal=my_newGoal(point=point_for_state, id=ids[1])), transitions={ 'succeeded': 'DRONE2-' + str(order[i + 1]), 'aborted': 'LAND_DRONE2', 'preempted': 'LAND_DRONE2' }) #make it infinit smach.StateMachine.add( 'DRONE2-' + str(order[-1]), SimpleActionState('drone2detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[-1]], id=ids[1])), transitions={ 'succeeded': 'DRONE2-' + str(order[0]), 'aborted': 'LAND_DRONE2', 'preempted': 'LAND_DRONE2' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE2', SimpleActionState('land_drone2', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[1])), transitions={'succeeded': 'LAND_DRONE2'}) drone3 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] Concurrence.add('DRONE3-', drone3) # Open the container with drone3: #add each state order = (1, 2, 3, 4, 5, 6, 7, 0) for i in range(7): point_for_state = Point() point_for_state.x = my_points[order[i]].x point_for_state.y = my_points[order[i]].y if i <= 4: point_for_state.z = my_points[order[i]].z + (i * 0.1) else: point_for_state.z = my_points[order[i]].z - (i * 0.1) + 0.8 StateMachine.add('DRONE3-' + str(order[i]), SimpleActionState( 'drone3detect_perimeter', my_newAction, goal=my_newGoal(point=point_for_state, id=ids[2])), transitions={ 'succeeded': 'DRONE3-' + str(order[i + 1]), 'aborted': 'LAND_DRONE3', 'preempted': 'LAND_DRONE3' }) #make it infinit smach.StateMachine.add( 'DRONE3-' + str(order[-1]), SimpleActionState('drone3detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[-1]], id=ids[2])), transitions={ 'succeeded': 'DRONE3-' + str(order[0]), 'aborted': 'LAND_DRONE3', 'preempted': 'LAND_DRONE3' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE3', SimpleActionState('land_drone3', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[2])), transitions={'succeeded': 'LAND_DRONE3'}) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm0.execute) smach_thread.start()
def main(): rospy.init_node('hcr_state_node') sm_hcr = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) """ Initialize userdata """ # target and current positions pose_init = geometry_msgs.msg.PoseStamped() pose_init.pose.position.x = 0 pose_init.pose.position.y = 0 pose_init.pose.position.z = 0 pose_init.pose.orientation.x = 0 pose_init.pose.orientation.y = 0 pose_init.pose.orientation.z = 0 pose_init.pose.orientation.w = 1 sm_hcr.userdata.pose_base_cur = geometry_msgs.msg.PoseStamped() sm_hcr.userdata.pose_base_tar = geometry_msgs.msg.PoseStamped() sm_hcr.userdata.pose_object_cur = geometry_msgs.msg.PoseStamped() sm_hcr.userdata.pose_object_tar = geometry_msgs.msg.PoseStamped() sm_hcr.userdata.pose_base_cur = pose_init sm_hcr.userdata.pose_base_cur.header.frame_id = "map" sm_hcr.userdata.pose_base_tar = pose_init sm_hcr.userdata.pose_base_tar.header.frame_id = "map" sm_hcr.userdata.pose_object_cur = pose_init sm_hcr.userdata.pose_object_tar = pose_init # Flags (flg) sm_hcr.userdata.flg_move_detect_done = False sm_hcr.userdata.flg_move_search_done = False sm_hcr.userdata.flg_object_grasp_done = False sm_hcr.userdata.flg_object_place_done = False sm_hcr.userdata.flg_move_new_goal = False # List of detected frames (type class Frame) for each run sm_hcr.userdata.list_detected_frames_tmp = [] sm_hcr.userdata.list_detected_frames_1 = [] sm_hcr.userdata.list_detected_frames_2 = [] sm_hcr.userdata.list_detected_frames_3 = [] """ Initialize subscribers """ def _move_base_goal_cb(pose): sm_hcr.userdata.pose_base_tar = pose sm_hcr.userdata.flg_move_new_goal = True rospy.Subscriber('/move_base_simple/goal', geometry_msgs.msg.PoseStamped, _move_base_goal_cb) """ Initialize thread """ def thread_tf_target_object_pose(): while 1: br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = sm_hcr.userdata.pose_object_tar.header.frame_id t.child_frame_id = "target_object" t.transform.translation = sm_hcr.userdata.pose_object_tar.pose.position t.transform.rotation = sm_hcr.userdata.pose_object_tar.pose.orientation try: br.sendTransform(t) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo( "ERROR: thread_tf_target_object_pose: tf-exeption") with sm_hcr: ### LOGIC ### smach.StateMachine.add('LOGIC', logic.Logic(), transitions={ 'aborted': 'aborted', 'reset': 'RESET', 'move_detect': 'MOVE_DETECT', 'move_search': 'MOVE_SEARCH', 'object_grasp': 'OBJECT_GRASP', 'object_place': 'OBJECT_PLACE' }, remapping={ 'flg_reset': 'flg_reset', 'flg_score': 'flg_score', 'flg_move_detect_done': 'flg_move_detect_done', 'flg_move_search_done': 'flg_move_search_done', 'flg_object_grasp_done': 'flg_object_grasp_done', 'flg_object_place_done': 'flg_object_place_done' }) ### RESET ### smach.StateMachine.add('RESET', logic.Reset(), transitions={'succeeded': 'LOGIC'}, remapping={ 'flg_move_detect_done': 'flg_move_detect_done', 'flg_move_search_done': 'flg_move_search_done', 'flg_move_new_goal': 'flg_move_new_goal', 'flg_object_grasp_done': 'flg_object_grasp_done', 'flg_object_place_done': 'flg_object_place_done', 'pose_base_tar': 'pose_base_tar', 'list_detected_frames_tmp': 'list_detected_frames_tmp', 'list_detected_frames_1': 'list_detected_frames_1', 'list_detected_frames_2': 'list_detected_frames_2', 'list_detected_frames_3': 'list_detected_frames_3' }) ### SCORE ### smach.StateMachine.add('SCORE', logic.Score(), transitions={ 'succeeded': 'LOGIC', 'aborted': 'RESET' }, remapping={ 'list_detected_frames_1': 'list_detected_frames_1', 'list_detected_frames_2': 'list_detected_frames_2', 'list_detected_frames_3': 'list_detected_frames_3' }) ### MOVE_DETECT ### sm_MoveDetect = smach.StateMachine( outcomes=['succeeded', 'aborted'], input_keys=['pose_base_cur', 'list_detected_frames_tmp'], output_keys=[ 'pose_base_cur', 'list_detected_frames_tmp', 'list_detected_frames_1', 'flg_move_detect_done' ]) # sub Detect smach.StateMachine.add('MOVE_DETECT', sm_MoveDetect, { 'succeeded': 'SCORE', 'aborted': 'RESET' }) with sm_MoveDetect: smach.StateMachine.add('DETECT', measure.Detect(), transitions={'succeeded': 'STOP'}) smach.StateMachine.add( 'STOP', move.Stop(), transitions={'succeeded': 'MEASURE'}, remapping={'pose_base_cur': 'pose_base_cur'}) smach.StateMachine.add( 'MEASURE', measure.Measure(), transitions={'succeeded': 'MOVE_DETECT_EXIT'}, remapping={ 'pose_base_cur': 'pose_base_cur', 'list_detected_frames_tmp': 'list_detected_frames_tmp' }) smach.StateMachine.add('MOVE_DETECT_EXIT', logic.MoveDetectExit(), transitions={'succeeded': 'succeeded'}, remapping={ 'list_detected_frames_tmp': 'list_detected_frames_tmp', 'list_detected_frames_1': 'list_detected_frames_1', 'flg_move_detect_done': 'flg_move_detect_done' }) ### MOVE_SEARCH ### sm_MoveSearch = smach.StateMachine( outcomes=['succeeded', 'aborted'], input_keys=[ 'pose_base_tar', 'pose_base_cur', 'pose_object_tar', 'list_detected_frames_tmp' ], output_keys=[ 'pose_base_tar', 'pose_base_cur', 'list_detected_frames_tmp', 'list_detected_frames_2', 'flg_move_search_done' ]) smach.StateMachine.add('MOVE_SEARCH', sm_MoveSearch, { 'succeeded': 'SCORE', 'aborted': 'RESET' }) with sm_MoveSearch: smach.StateMachine.add( 'BASEPOSE_FINDER', logic.BasePoseFinder(), transitions={'succeeded': 'MOVE'}, # transitions={'succeeded':'MEASURE'}, remapping={ 'pose_object_tar': 'pose_object_tar', 'pose_base_tar': 'pose_base_tar' }) smach.StateMachine.add('MOVE', move.Move(), transitions={'succeeded': 'MEASURE'}, remapping={ 'pose_base_tar': 'pose_base_tar', 'pose_base_cur': 'pose_base_cur' }) smach.StateMachine.add( 'MEASURE', measure.Measure(), transitions={'succeeded': 'MOVE_SEARCH_EXIT'}, remapping={ 'pose_base_cur': 'pose_base_cur', 'list_detected_frames_tmp': 'list_detected_frames_tmp' }) smach.StateMachine.add('MOVE_SEARCH_EXIT', logic.MoveSearchExit(), transitions={'succeeded': 'succeeded'}, remapping={ 'list_detected_frames_tmp': 'list_detected_frames_tmp', 'list_detected_frames_2': 'list_detected_frames_2', 'flg_move_search_done': 'flg_move_search_done' }) ### OBJECT_GRASP ### sm_ObjectGrasp = smach.StateMachine( outcomes=['succeeded', 'aborted'], input_keys=['flg_object_grasp_done', 'pose_object_tar'], output_keys=['flg_object_grasp_done']) smach.StateMachine.add('OBJECT_GRASP', sm_ObjectGrasp, { 'succeeded': 'LOGIC', 'aborted': 'RESET' }) with sm_ObjectGrasp: smach.StateMachine.add( 'PICK', grasp.Pick(), transitions={'succeeded': 'OBJECT_GRASP_EXIT'}, remapping={'pose_object_tar': 'pose_object_tar'}) smach.StateMachine.add( 'OBJECT_GRASP_EXIT', logic.ObjectGraspExit(), transitions={'succeeded': 'succeeded'}, remapping={'flg_object_grasp_done': 'flg_object_grasp_done'}) ### OBJECT_PLACE ### sm_ObjectPlace = smach.StateMachine( outcomes=['succeeded', 'aborted'], input_keys=[ 'pose_base_cur', 'pose_base_tar', 'flg_object_place_done' ], output_keys=['pose_base_cur', 'flg_object_place_done']) smach.StateMachine.add('OBJECT_PLACE', sm_ObjectPlace, { 'succeeded': 'LOGIC', 'aborted': 'RESET' }) with sm_ObjectPlace: smach.StateMachine.add('MOVE', move.Move(), transitions={'succeeded': 'PLACE'}, remapping={ 'pose_base_tar': 'pose_base_tar', 'pose_base_cur': 'pose_base_cur' }) smach.StateMachine.add( 'PLACE', grasp.Place(), transitions={'succeeded': 'OBJECT_PLACE_EXIT'}) smach.StateMachine.add( 'OBJECT_PLACE_EXIT', logic.ObjectPlaceExit(), transitions={'succeeded': 'succeeded'}, remapping={'flg_object_place_done': 'flg_object_place_done'}) # # attach a SMACH introspection server sis = smach_ros.IntrospectionServer('hcr_state_machine', sm_hcr, '/HCR_State_Machine') sis.start() # set preempt handler smach_ros.set_preempt_handler(sm_hcr) # Execute SMACH tree in a separate thread so that we can ctrl-c the script thread_tf_tarBasePose = threading.Thread( target=thread_tf_target_object_pose) thread_tf_tarBasePose.start() thread_smach = threading.Thread(target=sm_hcr.execute) thread_smach.start() # signal handler rospy.spin()
def polygonial(): ids = [4, 5, 6] #define the differents points my_points = [Point(), Point(), Point(), Point()] my_points[0].x = 0.9 - 0.4 my_points[0].y = 0.0 - 0.4 my_points[0].z = 0.9 my_points[1].x = 0.9 - 0.4 my_points[1].y = 0.9 - 0.4 my_points[1].z = 0.9 my_points[2].x = 0.0 - 0.4 my_points[2].y = 0.9 - 0.4 my_points[2].z = 0.9 my_points[3].x = 0.0 - 0.4 my_points[3].y = 0.0 - 0.4 my_points[3].z = 0.9 # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) #progressively add drones with sm0: # StateMachine.add('drone1-1', # SimpleActionState('drone1detect_perimeter', # my_newAction, goal = my_newGoal(point = my_points[0], id = ids[0] )), # transitions={'succeeded' : 'drone1-2'}) # StateMachine.add('drone1-2', # SimpleActionState('drone1detect_perimeter', # my_newAction, goal = my_newGoal(point = my_points[1], id = ids[0] )), # transitions={'succeeded' : 'drone2-1'}) # StateMachine.add('drone2-1', # SimpleActionState('drone2detect_perimeter', # my_newAction, goal = my_newGoal(point = my_points[0], id = ids[1] )), # transitions={'succeeded' : 'drone1-3'}) StateMachine.add('drone1-3', SimpleActionState('drone1detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[2], id=ids[0])), transitions={ 'succeeded': 'drone2-2', 'aborted': 'land_all' }) StateMachine.add('drone2-2', SimpleActionState('drone2detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[1], id=ids[1])), transitions={ 'succeeded': 'drone3-1', 'aborted': 'land_all' }) StateMachine.add('drone3-1', SimpleActionState('drone3detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[0], id=ids[2])), transitions={ 'succeeded': 'infinit_loop', 'aborted': 'land_all' }) land_sm = Concurrence(['succeeded', 'aborted', 'preempted'], 'succeeded') StateMachine.add('land_all', land_sm) with land_sm: # DUPLICATA FOR HIGH LEVEL # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE1', SimpleActionState('land_drone1', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[0]))) # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE2', SimpleActionState('land_drone2', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[1]))) # #Land Drone If Aborted Concurrence.add( 'LAND_DRONE3', SimpleActionState('land_drone3', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[2]))) ############################################ sm1 = Concurrence( ['succeeded', 'aborted', 'preempted'], 'succeeded', #child_termination_cb = lambda so: True, #outcome_map = { #'succeeded':{'WAIT_FOR_CLEAR':'valid'}, #'aborted':{'DRONE1':'aborted'}} ) StateMachine.add('infinit_loop', sm1) with sm1: drone1 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] Concurrence.add('DRONE1', drone1) # Open the container with drone1: #add each state order = (3, 0, 1, 2) for i in range(3): StateMachine.add( 'DRONE1-' + str(order[i]), SimpleActionState('drone1detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[i]], id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[i + 1]), 'aborted': 'LAND_DRONE1' }) #make it infinit smach.StateMachine.add( 'DRONE1-' + str(2), SimpleActionState( 'drone1detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[order[3]], id=ids[0])), transitions={ 'succeeded': 'DRONE1-' + str(order[0]), 'aborted': 'LAND_DRONE1' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE1', SimpleActionState('land_drone1', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[0])), transitions={'succeeded': 'LAND_DRONE1'}) drone2 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] Concurrence.add('DRONE2', drone2) # Open the container with drone2: #add each state order = (2, 3, 0, 1) for i in range(3): StateMachine.add( 'DRONE2-' + str(order[i]), SimpleActionState('drone2detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[i]], id=ids[1])), transitions={ 'succeeded': 'DRONE2-' + str(order[i + 1]), 'aborted': 'LAND_DRONE2' }) #make it infinit smach.StateMachine.add( 'DRONE2-' + str(1), SimpleActionState( 'drone2detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[order[3]], id=ids[1])), transitions={ 'succeeded': 'DRONE2-' + str(order[0]), 'aborted': 'LAND_DRONE2' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE2', SimpleActionState('land_drone2', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[1])), transitions={'succeeded': 'LAND_DRONE2'}) drone3 = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # ['succeeded','aborted','preempted'] Concurrence.add('DRONE3-', drone3) # Open the container with drone3: #add each state order = (1, 2, 3, 0) for i in range(3): StateMachine.add( 'DRONE3-' + str(order[i]), SimpleActionState('drone3detect_perimeter', my_newAction, goal=my_newGoal( point=my_points[order[i]], id=ids[2])), transitions={ 'succeeded': 'DRONE3-' + str(order[i + 1]), 'aborted': 'LAND_DRONE3' }) #make it infinit smach.StateMachine.add( 'DRONE3-' + str(0), SimpleActionState( 'drone3detect_perimeter', my_newAction, goal=my_newGoal(point=my_points[order[3]], id=ids[2])), transitions={ 'succeeded': 'DRONE3-' + str(order[0]), 'aborted': 'LAND_DRONE3' }) # #Land Drone If Aborted smach.StateMachine.add( 'LAND_DRONE3', SimpleActionState('land_drone3', my_newAction, goal=my_newGoal(point=my_points[3], id=ids[2])), transitions={'succeeded': 'LAND_DRONE3'}) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm0.execute) smach_thread.start()
def main(): """ This is a State Machine for a Robot Dog name wheely. The main function initializes the ros node, subscribes to the topic: robot's odometry topic ("/odom"), laser scan data (/scan), and the robot's camera image topic ("camera1/image_raw/compressed"), and creates a publisher for the robot's velocity commands on the topic ("/cmd_vel"), a publisher for cancelling the goal prvided to the move_base server ("/move_base/cancel"). This function also intializes the "detectBallFlag", "expLiteFlag", "playflag", "nearhuman", "room", "ballToBeFound" parameters in the ROS Parameter Server. This function also defines the state machines and adds four states/behaviors to the container. """ global pub_cmd_vel, pub_cancel rospy.set_param("expLiteFlag", 0) rospy.set_param("playflag", 0) rospy.set_param("nearHuman", 0) rospy.set_param("room", "") rospy.set_param("ballToBeFound", "") sub_camera = rospy.Subscriber("/camera1/image_raw/compressed", CompressedImage, imageCallback, queue_size=1) sub_odom = rospy.Subscriber("/odom", Odometry, robotPos) sub_laser = rospy.Subscriber('/scan', LaserScan, clbk_laser) pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1) pub_cancel = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1) rospy.init_node('Assignment_3_State_Machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['container_interface']) # Open the container with sm: # Add states to the container smach.StateMachine.add('NORMAL', Normal(), transitions={ 'after finishing normal behavior': 'SLEEP', 'speech command:play': 'PLAY' }) smach.StateMachine.add( 'SLEEP', Sleep(), transitions={'after finishing sleep time': 'NORMAL'}) smach.StateMachine.add('PLAY', Play(), transitions={ 'after finishing play time': 'NORMAL', 'ball not already detected': 'FIND' }) smach.StateMachine.add('FIND', Find(), transitions={ 'required ball found': 'PLAY', 'required ball not found': 'PLAY' }) # Create and start the introspection server for visualization. We # can visualize this is smach viewer sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute the state machine smach_ros.set_preempt_handler(sm) outcome = sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
def polygonial(): # Construct static goals polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0) polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded','aborted','preempted']) # Open the container with sm0: # Reset turtlesim StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty), {'succeeded':'SPAWN'}) # Create a second turtle StateMachine.add('SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request = turtlesim.srv.SpawnRequest(5.0,2.0,0.0,'turtle2')), {'succeeded':'PRECISE'}) # fib_goal = FibonacciGoal(order=20) # fib_fullaction = SimpleActionState('fibonacci', FibonacciAction, # goal=fib_goal) # # StateMachine.add('FIBONACCI', # fib_fullaction, # transitions={'succeeded':'PRECISE'}) goto_goal = FibonacciGoal(order=20) goto_fullaction = SimpleActionState('detect_perimeter', FibonacciAction, goal=goto_goal) StateMachine.add('PRECISE', goto_fullaction, transitions={'succeeded':'TELEPORT1'}) # Teleport turtle 1 StateMachine.add('TELEPORT1', ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)), {'succeeded':'DRAW_SHAPES'}) print("hi") # Draw some polygons shapes_cc = Concurrence( outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}}) StateMachine.add('DRAW_SHAPES',shapes_cc) with shapes_cc: # Draw a large polygon with the first turtle Concurrence.add('BIG', SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction, goal = polygon_big)) # Draw a small polygon with the second turtle small_shape_sm = StateMachine(outcomes=['succeeded','aborted','preempted']) Concurrence.add('SMALL',small_shape_sm) with small_shape_sm: # Teleport turtle 2 StateMachine.add('TELEPORT2', ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)), {'succeeded':'DRAW_WITH_MONITOR'}) # Construct a concurrence for the shape action and the monitor draw_monitor_cc = Concurrence( ['succeeded','aborted','preempted','interrupted'], 'aborted', child_termination_cb = lambda so: True, outcome_map = { 'succeeded':{'DRAW':'succeeded'}, 'preempted':{'DRAW':'preempted','MONITOR':'preempted'}, 'interrupted':{'MONITOR':'invalid'}}) StateMachine.add('DRAW_WITH_MONITOR', draw_monitor_cc, {'interrupted':'WAIT_FOR_CLEAR'}) with draw_monitor_cc: Concurrence.add('DRAW', SimpleActionState('togoal', FibonacciAction, goal=goto_goal)) def turtle_far_away(ud, msg): """Returns True while turtle pose in msg is at least 1 unit away from (2,5)""" if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0: return True return False Concurrence.add('MONITOR', MonitorState('/turtle1/pose',turtlesim.msg.Pose, cond_cb = turtle_far_away)) StateMachine.add('WAIT_FOR_CLEAR', MonitorState('/turtle1/pose',turtlesim.msg.Pose, cond_cb = lambda ud,msg: not turtle_far_away(ud,msg)), {'valid':'WAIT_FOR_CLEAR','invalid':'TELEPORT2'}) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target = sm0.execute) smach_thread.start()
Concurrence.add('INSPECTION', INSPECTION()) StateMachine.add('INSPECTION_ACTION', sm_inspection, transitions={ 'success': 'CHOOSE_ACTION', 'fail': 'stop' }) # Attach a SMACH introspection server sis = IntrospectionServer('baxter_SMACH_introspection', sm, '/BAXTER_DEMO') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm.execute) smach_thread.start() # Signal handler (wait for CTRL+C) rospy.spin() rospy.on_shutdown(clean_shutdown) except rospy.ROSInterruptException: # Baxter screen output image_pub.publish(msg_sleeping)