def main(): rospy.init_node('smach_example_actionlib_service_state') # Register a gripper service s = rospy.Service('gripper_srv', GripperSrv, gripper_srv) # Create a SMACH state machine sm0 = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Set userdata sm0.userdata.max_effort = 9.0 sm0.userdata.position = Point() sm0.userdata.gripper_input = 9.0 # Open the container with sm0: # Add states to the container # Empty request message smach.StateMachine.add( 'TRIGGER_GRIPPER_EMPTY_REQUEST', smach_ros.ServiceState('gripper_srv', GripperSrv), transitions={'succeeded': 'TRIGGER_GRIPPER_FIXED_REQUEST'}) # Fixed request message smach.StateMachine.add( 'TRIGGER_GRIPPER_FIXED_REQUEST', smach_ros.ServiceState('gripper_srv', GripperSrv, request=GripperSrvRequest(4.0, Point())), transitions={'succeeded': 'TRIGGER_GRIPPER_USER_DATA_REQUEST'}) # Request from user data smach.StateMachine.add( 'TRIGGER_GRIPPER_USER_DATA_REQUEST', smach_ros.ServiceState('gripper_srv', GripperSrv, request_slots=['max_effort', 'position']), transitions={'succeeded': 'TRIGGER_GRIPPER_REQUEST_CALLBACK'}) # Request callback @smach.cb_interface(input_keys=['gripper_input']) def gripper_request_cb(userdata, request): gripper_request = GripperSrvRequest() gripper_request.position.x = 2.0 gripper_request.max_effort = userdata.gripper_input return gripper_request smach.StateMachine.add('TRIGGER_GRIPPER_REQUEST_CALLBACK', smach_ros.ServiceState( 'gripper_srv', GripperSrv, request_cb=gripper_request_cb, input_keys=['gripper_input']), transitions={'succeeded': 'succeeded'}) # Execute SMACH plan outcome = sm0.execute() rospy.signal_shutdown('All done.')
def PlaceObject(attempts=3): """ Place a given object, retrying up to a given number of times """ it = smach.Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_name', 'support_surf', 'place_pose'], output_keys=[], it=lambda: range(0, attempts), it_label='attempt', exhausted_outcome='aborted') with it: sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted', 'continue'], input_keys=['object_name', 'support_surf', 'place_pose'], output_keys=[]) with sm: smach.StateMachine.add( 'PlaceObject', smach_ros.SimpleActionState( 'place_object', thorp_msgs.PlaceObjectAction, goal_slots=['object_name', 'support_surf', 'place_pose'], result_slots=[]), remapping={ 'object_name': 'object_name', 'support_surf': 'support_surf', 'place_pose': 'place_pose' }, transitions={ 'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'ClearOctomap' }) smach.StateMachine.add('ClearOctomap', smach_ros.ServiceState( 'clear_octomap', std_srvs.Empty), transitions={ 'succeeded': 'continue', 'preempted': 'preempted', 'aborted': 'aborted' }) # TODOs: # - we should open the gripper, in case we have picked an object # - check error and, if collision between parts of the arm, move a bit the arm --> not enough info # - this doesn't make too much sense as a loop... better try all our tricks and exit # - can I reuse the same for place and MoveToTarget??? smach.Iterator.set_contained_state('', sm, loop_outcomes=['continue']) return it
def main(): rospy.init_node('state_machine_node') # Create a TOP level SMACH state machine top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted']) # Open the container with top_sm: # ===================================== SarThreads ===================================== # Callback for custom outcomes from SarThreads # def out_cb(outcome_map): # To be completed return 'aborted' # Create a Concurrence container sarthreads_concurrence = smach.Concurrence( outcomes=['missionAbort', 'aborted'], default_outcome='missionAbort', child_termination_cb=lambda so: True, outcome_cb=out_cb ) # Open the container with sarthreads_concurrence: # ===================================== SarBehavior ===================================== # Create a State Machine container sarbehavior_sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted'] ) # Open the container with sarbehavior_sm: # ===================================== IdleThreads ===================================== # Callback for custom outcomes from IdleThreads # def out_cb(outcome_map): # To be completed return 'aborted' # Create a Concurrence container idlethreads_concurrence = smach.Concurrence( outcomes=['missionStart', 'aborted'], default_outcome='missionStart', child_termination_cb=lambda so: True, outcome_cb=out_cb ) # Open the container with idlethreads_concurrence: # ADD Idle to IdleThreads # smach.Concurrence.add('Idle', # NOT_SUPPORTED ) # ADD IdleEventMonitoring to IdleThreads # smach.Concurrence.add('IdleEventMonitoring', smach_ros.MonitorState('bridge/events/mission_start', SimpleEvent, cond_cb=lambda ud, msg: False) ) # ===================================== IdleThreads END ===================================== # ADD IdleThreads to SarBehavior # smach.StateMachine.add('IdleThreads', idlethreads_concurrence, transitions={'missionStart':'TakeOff'} ) # ADD TakeOff to SarBehavior # smach.StateMachine.add('TakeOff', smach_ros.SimpleActionState('cmd/takeoff', TakeOffAction, goal=TakeOffGoal(1.5)), transitions={'succeeded':'Coverage'} ) # ADD Coverage to SarBehavior # smach.StateMachine.add('Coverage', smach_ros.SimpleActionState('uav_coverage', CoverageAction), transitions={'succeeded':'SelectRover'} ) # ADD SelectRover to SarBehavior # smach.StateMachine.add('SelectRover', smach_ros.SimpleActionState('cmd/assign_task', AssignTaskAction, goal_slots=['target_id', 'pose']), transitions={'succeeded':'Tracking', 'aborted':'SelectRover'} ) # ADD Tracking to SarBehavior # smach.StateMachine.add('Tracking', smach_ros.SimpleActionState('uav_tracking', TrackingAction, goal_slots=['target', 'cps']), transitions={'succeeded':'Coverage', 'aborted':'LocalCoverage'} ) # ADD LocalCoverage to SarBehavior # smach.StateMachine.add('LocalCoverage', smach_ros.SimpleActionState('uav_local_coverage', CoverageAction), transitions={'aborted':'Coverage', 'succeeded':'Tracking'} ) # ===================================== SarBehavior END ===================================== # ADD SarBehavior to SarThreads # smach.Concurrence.add('SarBehavior', sarbehavior_sm) # ADD AbortEventMonitoring to SarThreads # smach.Concurrence.add('AbortEventMonitoring', smach_ros.MonitorState('bridge/events/mission_abort', SimpleEvent, cond_cb=lambda ud, msg: False) ) # ===================================== SarThreads END ===================================== # ADD SarThreads to TOP state # smach.StateMachine.add('SarThreads', sarthreads_concurrence, transitions={'missionAbort':'MissionAbort'} ) # ===================================== MissionAbort ===================================== # Create a State Machine container missionabort_sm = smach.StateMachine( outcomes=['succeeded'] ) # Open the container with missionabort_sm: # ADD Land to MissionAbort # smach.StateMachine.add('Land', smach_ros.ServiceState('cmd/land', Empty), transitions={} ) # ===================================== MissionAbort END ===================================== # ADD MissionAbort to TOP state # smach.StateMachine.add('MissionAbort', missionabort_sm, transitions={'succeeded':'SarThreads'} ) # Create and start the introspection server (uncomment if needed) # sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP') # sis.start() # Execute SMACH plan outcome = top_sm.execute() # Wait for ctrl-c to stop the application rospy.spin()
transitions={ 'succeeded': 'ObjectDetection', 'preempted': 'preempted', 'aborted': 'ObjectDetection' }) smach.StateMachine.add('FoldArmAndRelax', FoldArm(), transitions={ 'succeeded': 'RelaxArmAndStop', 'preempted': 'preempted', 'aborted': 'error' }) smach.StateMachine.add('RelaxArmAndStop', smach_ros.ServiceState('servos/relax_all', arbotix_srvs.Relax), transitions={ 'succeeded': 'stop', 'preempted': 'stop', 'aborted': 'error' }) # Construct action server wrapper for top-level sm to control it with keyboard commands asw = smach_ros.ActionServerWrapper('user_commands_action_server', thorp_msgs.UserCommandAction, wrapped_container=sm, succeeded_outcomes=['stop'], aborted_outcomes=['aborted'], preempted_outcomes=['error'], goal_key='user_command', feedback_key='ucmd_progress',
def ObjectDetection(): """ Object detection sub state machine; iterates over object_detection action state and recovery mechanism until an object is detected, it's preempted or there's an error (aborted outcome) """ class ObjDetectedCondition(smach.State): """ Check for the object detection result to retry if no objects where detected """ def __init__(self): smach.State.__init__( self, outcomes=['preempted', 'satisfied', 'fold_arm', 'retry'], input_keys=['od_attempt', 'object_names'], output_keys=['od_attempt']) def execute(self, userdata): if self.preempt_requested(): self.service_preempt() return 'preempted' if len(userdata.object_names) > 0: userdata.od_attempt = 0 return 'satisfied' userdata.od_attempt += 1 if userdata.od_attempt == 1: return 'fold_arm' return 'retry' sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['od_attempt', 'output_frame'], output_keys=['objects', 'object_names', 'support_surf']) with sm: smach.StateMachine.add('ClearOctomap', smach_ros.ServiceState('clear_octomap', std_srvs.Empty), transitions={ 'succeeded': 'ObjectDetection', 'preempted': 'preempted', 'aborted': 'ObjectDetection' }) smach.StateMachine.add( 'ObjectDetection', smach_ros.SimpleActionState( 'object_detection', thorp_msgs.DetectObjectsAction, goal_slots=['output_frame'], result_slots=['objects', 'object_names', 'support_surf']), remapping={ 'output_frame': 'output_frame', 'object_names': 'object_names', 'support_surf': 'support_surf' }, transitions={ 'succeeded': 'ObjDetectedCondition', 'preempted': 'preempted', 'aborted': 'aborted' }) smach.StateMachine.add('ObjDetectedCondition', ObjDetectedCondition(), remapping={'object_names': 'object_names'}, transitions={ 'satisfied': 'succeeded', 'preempted': 'preempted', 'fold_arm': 'FoldArm', 'retry': 'ClearOctomap' }) smach.StateMachine.add('FoldArm', FoldArm(), transitions={ 'succeeded': 'ClearOctomap', 'preempted': 'preempted', 'aborted': 'ClearOctomap' }) return sm
def main(): rospy.init_node('state_machine_node') if not rospy.has_param('~altitude'): rospy.logerr('Altitude not specified, cannot perform simulation!') return # Create a TOP level SMACH state machine top_sm = smach.StateMachine(['succeeded', 'preempted', 'aborted']) # Open the container with top_sm: # ===================================== SarThreads ===================================== # Callback for custom outcomes from SarThreads def out_cb(outcome_map): if outcome_map['AbortEventMonitoring'] == 'invalid': rospy.loginfo('Returning missionAbort Event') return 'missionAbort' return 'aborted' # Create a Concurrence container sarthreads_concurrence = smach.Concurrence( outcomes=['missionAbort', 'aborted'], default_outcome='missionAbort', child_termination_cb=lambda so: True, outcome_cb=out_cb) # Open the container with sarthreads_concurrence: # ===================================== SarBehavior ===================================== # Create a State Machine container sarbehavior_sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted']) # Open the container with sarbehavior_sm: # ADD Idle to SarBehavior # smach.StateMachine.add('Idle', Idle(), transitions={'succeeded': 'Takeoff'}) # ADD Takeoff to SarBehavior # smach.StateMachine.add('Takeoff', smach_ros.SimpleActionState( 'cmd/takeoff', TakeoffAction, goal=TakeoffGoal( rospy.get_param('~altitude'))), transitions={'succeeded': 'Coverage'}) # ADD Coverage to SarBehavior # smach.StateMachine.add( 'Coverage', smach_ros.SimpleActionState( 'uav_random_coverage', CoverageAction, goal=CoverageGoal(rospy.get_param('~altitude')), result_slots=['target_id', 'target_pose']), transitions={'succeeded': 'Tracking'}, remapping={ 'target_id': 'target_id', 'target_pose': 'target_pose' }) # ===================================== SelectRoverThreads ===================================== # Callback for custom outcomes def selectrover_outcb(outcome_map): if outcome_map['LostEventMonitoring'] == 'invalid': rospy.loginfo('Returning target_lost Event') return 'target_lost' return outcome_map['TaskAllocation'] # Create a Concurrence container selectrover_concurrence = smach.Concurrence( input_keys=['target_id', 'target_pose'], outcomes=['succeeded', 'aborted', 'target_lost'], default_outcome='aborted', child_termination_cb=lambda so: True, outcome_cb=selectrover_outcb) # Open the container with selectrover_concurrence: # ADD TaskAllocation to SelectRoverThreads # smach.Concurrence.add( 'TaskAllocation', smach_ros.SimpleActionState( 'cmd/task_allocation_auction', TaskAllocationAction, goal_slots=['task_id', 'task_pose'], result_slots=['task_id', 'winner', 'task_pose']), remapping={ 'task_id': 'target_id', 'task_pose': 'target_pose' }) # ADD LostEventMonitoring to SelectRoverThreads # smach.Concurrence.add( 'LostEventMonitoring', smach_ros.MonitorState('target_lost', TargetPositionEvent, cond_cb=lambda ud, msg: False)) # ===================================== SelectRoverThreads END ===================================== # ADD SelectRoverThreads to SarBehavior # smach.StateMachine.add('SelectRover', selectrover_concurrence, transitions={ 'succeeded': 'Tracking', 'aborted': 'SelectRover', 'target_lost': 'LocalCoverage' }) def tracking_goal_cb(userdata, goal): tracking_goal = TrackingGoal() tracking_goal.target = userdata.target tracking_goal.altitude = rospy.get_param('~altitude') return tracking_goal # ADD Tracking to SarBehavior # smach.StateMachine.add('Tracking', smach_ros.SimpleActionState( 'uav_simple_tracking', TrackingAction, input_keys=['target'], goal_cb=tracking_goal_cb), transitions={ 'succeeded': 'Coverage', 'aborted': 'LocalCoverage' }, remapping={'target': 'target_id'}) # ADD LocalCoverage to SarBehavior # smach.StateMachine.add( 'LocalCoverage', smach_ros.SimpleActionState( 'uav_local_coverage', CoverageAction, goal=CoverageGoal(rospy.get_param('~altitude')), result_slots=['target_id', 'target_pose']), transitions={ 'aborted': 'Coverage', 'succeeded': 'SelectRover' }, remapping={ 'target_id': 'target_id', 'target_pose': 'target_pose' }) # ===================================== SarBehavior END ===================================== # ADD SarBehavior to SarThreads # smach.Concurrence.add('SarBehavior', sarbehavior_sm) # ADD AbortEventMonitoring to SarThreads # smach.Concurrence.add( 'AbortEventMonitoring', smach_ros.MonitorState('bridge/events/mission_abort', SimpleEvent, cond_cb=lambda ud, msg: False)) # ===================================== SarThreads END ===================================== # ADD SarThreads to TOP state # smach.StateMachine.add('SarThreads', sarthreads_concurrence, transitions={'missionAbort': 'MissionAbort'}) # ===================================== MissionAbort ===================================== # Create a State Machine container missionabort_sm = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted']) # Open the container with missionabort_sm: # ADD Land to MissionAbort # smach.StateMachine.add('Land', smach_ros.ServiceState( 'cmd/land', std_srvs.srv.Empty), transitions={}) # ===================================== MissionAbort END ===================================== # ADD MissionAbort to TOP state # smach.StateMachine.add('MissionAbort', missionabort_sm, transitions={'succeeded': 'SarThreads'}) # Create and start the introspection server (uncomment if needed) sis = smach_ros.IntrospectionServer('smach_server', top_sm, '/SM_TOP') sis.start() # Execute SMACH plan outcome = top_sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
class Challenge1(): rospy.init_node('MBZIRC_ch1_state_machine') sm = smach.StateMachine(outcomes=['Done']) with sm: def exploration_response_cb(userdata, response): print "Exploration Response:", response.navResponse if response.navResponse == 'succeeded': return 'succeeded' else: return 'aborted' def landing_response_cb(userdata, response): print 'Landing Response', response.navResponse if response.navResponse == 'succeeded': return 'succeeded' else: return 'aborted' smach.StateMachine.add('INITIATING', initialization(), transitions={'succeeded': 'EXPLORATION'}) ############################################3 # call service with request 1 and userdata # ############################################3 smach.StateMachine.add( 'EXPLORATION', smach_ros.ServiceState( 'controllerService', navigation, request_slots=['srvtype'], #request_cb = exploration_request_cb, response_cb=exploration_response_cb, input_keys=['srvtype']), transitions={ 'succeeded': 'MARKER_DETECTION', 'aborted': 'MARKER_DETECTION', 'preempted': 'MARKER_DETECTION' }) ############################################3 # MArker Detection and tracking # in this state the drone will not perform RTL becuase it is build it to hover ############################################3 smach.StateMachine.add('MARKER_DETECTION', checkMarkerDetection(), transitions={ 'tracked': 'LANDING', 'explore': 'EXPLORATION' }) ############################################3 # call service with request 2 and userdata # ############################################3 smach.StateMachine.add( 'LANDING', ServiceState( 'controllerService', navigation, #request_cb = landing_request_cb, request_slots=['srvtype'], response_cb=landing_response_cb, input_keys=['srvtype']), transitions={ 'succeeded': 'Done', 'aborted': 'MARKER_DETECTION', 'preempted': 'MARKER_DETECTION' }) #######################################################3 #Before the Done There should be a disarming state #######################################################3 #I should publish to the offboard control to change to manual mode and then to Disarm sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() rospy.spin() sis.stop()
def main(): rospy.init_node('drone_executive') sm_root = smach.StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with sm_root: # CONNECTING is the initial state, # if connected, goto LANDED state @smach.cb_interface(outcomes=['aborted']) def connect_resp_cb(userdata, response): if response.success == False: return 'aborted' else: return 'succeeded' req_connect = ail_mav.srv.ConnectMavRequest('/mavros', True) smach.StateMachine.add('CONNECTING', smach_ros.ServiceState( '/mav_interface_node/connect', ail_mav.srv.ConnectMav, req_connect, response_cb=connect_resp_cb), transitions={ 'succeeded': 'LANDED', 'aborted': 'CONNECTING' }) # Come to LANDED after CONNECTING or LANDING # as defined in class Landed, hold 3 seconds, goto TAKINGOFF smach.StateMachine.add('LANDED', Landed(), transitions={'takeoff_started': 'TAKINGOFF'}) # Come to TAKINGOFF after time of LANDED expires # if responded success, goto # if aborted, goto LANDING @smach.cb_interface(outcomes=['aborted']) def takeoff_resp_cb(userdata, response): if response.success == False: return 'aborted' else: return 'succeeded' req_takeoff = ail_mav.srv.TakeoffRequest(1.0) smach.StateMachine.add('TAKINGOFF', smach_ros.ServiceState( '/mav_interface_node/takeoff', ail_mav.srv.Takeoff, req_takeoff, response_cb=takeoff_resp_cb), transitions={ 'succeeded': 'FLYING_TO_WAITING_ZONE', 'aborted': 'LANDING' }) # Come to LANDING after TAKINGOFF aborted or FLYING_TO_LAND # goto LANDED afterwards smach.StateMachine.add( 'LANDING', smach_ros.ServiceState( '/mav_interface_node/land', ail_mav.srv.Land), #,ail_mav.srv.LandRequest.Empty), transitions={'succeeded': 'LANDED'}) #FLYING_TO_WAITING_ZONE goal = mav_state_machine.msg.trackingGoal() goal.orientation_final = 0.0 goal.pos_final.x = -1.0 goal.pos_final.y = -0.4 goal.pos_final.z = 1.8 smach.StateMachine.add('FLYING_TO_WAITING_ZONE', smach_ros.SimpleActionState( '/traj_plan_track_action_server', mav_state_machine.msg.trackingAction, goal), transitions={'succeeded': 'FLYING_TO_LAND'}) goal2 = mav_state_machine.msg.trackingGoal() goal2.orientation_final = 0.0 goal2.pos_final.x = 0.0 goal2.pos_final.y = 0.0 goal2.pos_final.z = 1.0 smach.StateMachine.add('FLYING_TO_LAND', smach_ros.SimpleActionState( '/traj_plan_track_action_server', mav_state_machine.msg.trackingAction, goal2), transitions={'succeeded': 'LANDING'}) sis = smach_ros.IntrospectionServer('intro_server', sm_root, '/SM_ROOT') sis.start() outcome = sm_root.execute() rospy.spin() sis.stop()
smach.StateMachine.add('ExecuteUserCommand', ExecuteUserCommand(), transitions={'start':'ObjectDetection', 'reset':'ObjectDetection', 'fold':'FoldArm', 'quit':'FoldArmAndQuit'}) # Object detection sub state machine; iterates over object_detection action state and recovery # mechanism until an object is detected, it's preempted or there's an error (aborted outcome) od_sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted'], input_keys = ['od_attempt', 'frame', 'obj_names', 'named_pose_target_type', 'arm_folded_named_pose'], output_keys = ['obj_names']) with od_sm: smach.StateMachine.add('ObjDetectionClearOctomap', smach_ros.ServiceState('clear_octomap', std_srv.Empty), transitions={'succeeded':'ObjectDetectionOnce', 'preempted':'preempted', 'aborted':'ObjectDetectionOnce'}) smach.StateMachine.add('ObjectDetectionOnce', smach_ros.SimpleActionState('object_detection', ObjectDetectionAction, goal_slots=['frame'], result_slots=['obj_names']), remapping={'frame':'frame', 'obj_names':'obj_names'}, transitions={'succeeded':'ObjDetectedCondition', 'preempted':'preempted', 'aborted':'aborted'})
sm = smach.StateMachine(outcomes=[ 'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED', 'ACTION_ERROR' ]) with sm: smach.StateMachine.add( 'CONDITION', smach_ros.MonitorState('/ros_cle_simulation/status', String, time_30secs_cb), { 'valid': 'CONDITION', 'invalid': 'ACTION', 'preempted': 'CONDITION_PREEMPTED' }) smach.StateMachine.add( 'ACTION', smach_ros.ServiceState('/gazebo/set_visual_properties', SetVisualProperties, request=SetVisualPropertiesRequest( 'right_vr_screen', 'body', 'screen_glass', 'material:script:name', 'Gazebo/Red')), { 'succeeded': 'FINISHED', 'aborted': 'ACTION_ERROR', 'preempted': 'ACTION_PREEMPTED' }) result = sm.execute() rospy.loginfo(result)
remapping={'new_pose': 'place_pose'}, transitions={'succeeded': 'INC_PLACE_HEIGHT'}) smach.StateMachine.add('INC_PLACE_HEIGHT', IncreasePlaceHeight(), transitions={'succeeded': 'continue'}) smach.Iterator.set_contained_state('', sc_sm, loop_outcomes=['continue']) smach.StateMachine.add('STACK_CUBES', sc_it, { 'succeeded': 'FOLD_ARM_AND_RELAX', 'aborted': 'error' }) smach.StateMachine.add('CLEAR_GRIPPER', smach_ros.ServiceState('clear_gripper', std_srvs.Empty), transitions={ 'succeeded': 'OBJECT_DETECTION', 'preempted': 'preempted', 'aborted': 'aborted' }) smach.StateMachine.add('FOLD_ARM', FoldArm(), transitions={ 'succeeded': 'OBJECT_DETECTION', 'preempted': 'preempted', 'aborted': 'OBJECT_DETECTION' }) smach.StateMachine.add('FOLD_ARM_AND_RELAX', FoldArm(), transitions={
def main(): rospy.init_node('ma_smach') sm_root = smach.StateMachine(['succeeded', 'aborted', 'preempted']) with sm_root: smach.StateMachine.add('RESET', smach_ros.ServiceState('reset', Empty), transitions={'succeeded': 'SPAWN'}) smach.StateMachine.add('SPAWN', smach_ros.ServiceState('spawn', Spawn, request=SpawnRequest( 1.0, 1.0, 0.0, 'mark')), transitions={ 'succeeded': 'TURTLE1', 'aborted': 'aborted', 'preempted': 'preempted' }) #key point turtlesim xytheta is float type smach.StateMachine.add('TURTLE1', smach_ros.ServiceState( 'turtle1/teleport_absolute', TeleportAbsolute, request=TeleportAbsoluteRequest( 5.0, 1.0, 0.0)), transitions={ 'succeeded': 'CON', 'aborted': 'aborted', 'preempted': 'preempted' }) # Create the sub SMACH state machine sm_con = smach.Concurrence(outcomes=['suc', 'abo', 'pre'], default_outcome='abo', outcome_map={ 'suc': { 'TURTLE1_SHAPE': 'succeeded', 'DRAW_S': 'succeed' }, 'pre': { 'DRAW_S': 'preempt', 'TURTLE1_SHAPE': 'preempted' } }) smach.StateMachine.add('CON', sm_con, transitions={ 'abo': 'CON', 'suc': 'succeeded', 'pre': 'preempted' }) with sm_con: smach.Concurrence.add( 'TURTLE1_SHAPE', smach_ros.SimpleActionState('turtle_shape', ShapeAction, goal=ShapeGoal(11, 4.0))) sm_draw = smach.StateMachine(['succeed', 'abort', 'preempt']) with sm_draw: smach.StateMachine.add('MONITOR_CLEAR', smach_ros.MonitorState( 'mark/pose', Pose, clear_cb), transitions={ 'valid': 'MONITOR_CLEAR', 'invalid': 'TELE', 'preempted': 'preempt' }) smach.StateMachine.add('TELE', smach_ros.ServiceState( 'mark/teleport_absolute', TeleportAbsolute, request=TeleportAbsoluteRequest( 5.0, 5.0, 0.0)), transitions={ 'succeeded': 'MARK_SHAPE', 'aborted': 'abort', 'preempted': 'preempt' }) ma_con = smach.Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'interrupted' ], default_outcome='aborted', outcome_map={ 'succeeded': { 'MONITOR': 'valid', 'DRAW': 'succeeded' }, 'preempted': { 'MONITOR': 'invalid', 'DRAW': 'preempted' }, 'interrupted': { 'MONITOR': 'invalid' } }, child_termination_cb=lambda state_outcomes: True) smach.StateMachine.add('MARK_SHAPE', ma_con, transitions={ 'interrupted': 'MONITOR_CLEAR', 'aborted': 'abort', 'preempted': 'preempt', 'succeeded': 'succeed' }) with ma_con: smach.Concurrence.add( 'DRAW', smach_ros.SimpleActionState('mark_shape', ShapeAction, goal=ShapeGoal(6, 3.0))) smach.Concurrence.add( 'MONITOR', smach_ros.MonitorState("mark/pose", Pose, cond_cb)) smach.Concurrence.add('DRAW_S', sm_draw) sis = smach_ros.IntrospectionServer('ma_smach', sm_root, '/SM_ROOT') sis.start() outcome = sm_root.execute() rospy.spin() sis.stop()
def define_for(self, robot): with self: def lock_piles_area_request_callback(userdata, request): request = SetBoolRequest() request.data = True return request def lock_piles_area_response_callback(userdata, response): return 'succeeded' if response.success else 'aborted' smach.StateMachine.add( 'LOCK_PILES_AREA', smach_ros.ServiceState( 'lock_pick_region', SetBool, request_cb=lock_piles_area_request_callback, response_cb=lock_piles_area_response_callback), transitions={ 'succeeded': 'GO_TO_PILES', 'aborted': 'SLEEP_AND_RETRY_LOCK_PILES_AREA' }) smach.StateMachine.add( 'SLEEP_AND_RETRY_LOCK_PILES_AREA', SleepAndRetry(3.0), transitions={'succeeded': 'LOCK_PILES_AREA'}) smach.StateMachine.add( 'GO_TO_PILES', GoTo().define_for(robot), remapping={'waypoint': 'above_pile_pose'}, transitions={'succeeded': 'FREE_WAITING_SPOT'}) def free_waiting_spot_request_callback(userdata, request): request = SetBoolRequest() request.data = False return request def free_waiting_spot_response_callback(userdata, response): return 'succeeded' smach.StateMachine.add( 'FREE_WAITING_SPOT', smach_ros.ServiceState( 'lock_wait_region', SetBool, request_cb=free_waiting_spot_request_callback, response_cb=free_waiting_spot_response_callback), transitions={'succeeded': 'PICK_ACTION'}) def pick_action_goal_callback(userdata, default_goal): goal = mbzirc_comm_objs.msg.PickGoal(color=userdata.color) return goal smach.StateMachine.add('PICK_ACTION', smach_ros.SimpleActionState( robot.url + 'pick_action', mbzirc_comm_objs.msg.PickAction, input_keys=['color'], goal_cb=pick_action_goal_callback), transitions={ 'succeeded': 'succeeded', 'aborted': 'BACK_TO_WAITING_AREA' }) smach.StateMachine.add('BACK_TO_WAITING_AREA', GoTo().define_for(robot), remapping={'waypoint': 'waiting_pose'}, transitions={'succeeded': 'FREE_PILES'}) def free_piles_request_callback(userdata, request): request = SetBoolRequest() request.data = False return request def free_piles_response_callback(userdata, response): return 'succeeded' smach.StateMachine.add( 'FREE_PILES', smach_ros.ServiceState( 'lock_pick_region', SetBool, request_cb=free_piles_request_callback, response_cb=free_piles_response_callback), transitions={'succeeded': 'aborted'}) return self
def define_for(self, robot): with self: def lock_wall_area_request_callback(userdata, request): request = SetBoolRequest() request.data = True return request def lock_wall_area_response_callback(userdata, response): return 'succeeded' if response.success else 'aborted' smach.StateMachine.add( 'LOCK_WALL_AREA', smach_ros.ServiceState( 'lock_place_region', SetBool, request_cb=lock_wall_area_request_callback, response_cb=lock_wall_area_response_callback), transitions={ 'succeeded': 'GO_SEGMENT_TO_THE_LEFT', 'aborted': 'SLEEP_AND_RETRY_LOCK_WALL_AREA' }) smach.StateMachine.add('SLEEP_AND_RETRY_LOCK_WALL_AREA', SleepAndRetry(1.0), transitions={'succeeded': 'LOCK_WALL_AREA'}) smach.StateMachine.add( 'GO_SEGMENT_TO_THE_LEFT', GoTo().define_for(robot), remapping={'waypoint': 'segment_to_the_left_pose'}, transitions={'succeeded': 'FREE_PILES'}) def free_piles_request_callback(userdata, request): request = SetBoolRequest() request.data = False return request def free_piles_response_callback(userdata, response): return 'succeeded' smach.StateMachine.add( 'FREE_PILES', smach_ros.ServiceState( 'lock_pick_region', SetBool, request_cb=free_piles_request_callback, response_cb=free_piles_response_callback), transitions={'succeeded': 'PLACE_ACTION'}) def place_action_goal_callback(userdata, default_goal): goal = mbzirc_comm_objs.msg.PlaceGoal( wall_y_offset=userdata.segment_offset) return goal smach.StateMachine.add('PLACE_ACTION', smach_ros.SimpleActionState( robot.url + 'place_action', mbzirc_comm_objs.msg.PlaceAction, input_keys=['segment_offset'], goal_cb=place_action_goal_callback), transitions={ 'succeeded': 'BACK_TO_WAITING_AREA_SUCCEEDED', 'aborted': 'BACK_TO_WAITING_AREA_FAILED' }) smach.StateMachine.add( 'BACK_TO_WAITING_AREA_SUCCEEDED', GoTo().define_for(robot), remapping={'waypoint': 'waiting_pose'}, transitions={'succeeded': 'FREE_WALL_SUCCEEDED'}) def free_wall_request_callback(userdata, request): request = SetBoolRequest() request.data = False return request def free_wall_response_callback(userdata, response): return 'succeeded' smach.StateMachine.add( 'FREE_WALL_SUCCEEDED', smach_ros.ServiceState( 'lock_place_region', SetBool, request_cb=free_wall_request_callback, response_cb=free_wall_response_callback), transitions={'succeeded': 'succeeded'}) smach.StateMachine.add( 'BACK_TO_WAITING_AREA_FAILED', GoTo().define_for(robot), remapping={'waypoint': 'waiting_pose'}, transitions={'succeeded': 'FREE_WALL_FAILED'}) smach.StateMachine.add( 'FREE_WALL_FAILED', smach_ros.ServiceState( 'lock_place_region', SetBool, request_cb=free_wall_request_callback, response_cb=free_wall_response_callback), transitions={'succeeded': 'aborted'}) return self