def main(): rospy.init_node('align_with_workspace_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED','OVERALL_PREEMPTED'], input_keys = ['align_with_workspace_goal', 'align_with_workspace_result'], output_keys = ['align_with_workspace_feedback', 'align_with_workspace_result']) with sm: smach.StateMachine.add('ALIGN_BASE', SetupAlignWithWorkspace(), transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'SET_ACTION_LIB_FAILURE', 'preempted':'OVERALL_PREEMPTED'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), #transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'}) transitions={'succeeded':'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper( server_name = 'align_with_workspace_server', action_spec = AlignWithWorkspaceAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['OVERALL_PREEMPTED'], goal_key = 'align_with_workspace_goal', feedback_key = 'align_with_workspace_feedback', result_key = 'align_with_workspace_result') asw.run_server() rospy.spin()
def main(): rospy.init_node('skill_name_server') # Construct state machine sm = SkillName() # Smach viewer sis = smach_ros.IntrospectionServer('skill_name_smach_viewer', sm, '/SKILL_NAME_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name='skill_name_server', action_spec=SkillNameAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='S_goal', feedback_key='skill_name_feedback', result_key='skill_name_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('pick_object') sm = smach.StateMachine(outcomes=['picked', 'pick_failed', 'preempted', 'error'], input_keys=['goal'], output_keys=['feedback', 'result']) with sm: sm.userdata.pose_arm_default = geometry_msgs.PoseStamped() sm.userdata.pose_arm_default.header.stamp = rospy.Time.now() sm.userdata.pose_arm_default.header.frame_id = "/base_footprint" sm.userdata.pose_arm_default.pose.position.x = 0.1 sm.userdata.pose_arm_default.pose.position.y = 0.0 sm.userdata.pose_arm_default.pose.position.z = 0.5 sm.userdata.pose_arm_default.pose.orientation.w = 1.0 sm.userdata.result = pick_and_place_msgs.PickObjectResult() smach.StateMachine.add('ParseGoal', ParseGoal(), remapping={'goal':'goal', 'feedback':'feedback', 'object_pose':'object_pose'}, transitions={'parsed':'PickObject'}) sm_pick = pick_object_sm.createSM() smach.StateMachine.add('PickObject', sm_pick, remapping={'object_name':'object_name', 'object_pose':'object_pose', 'pose_arm_default':'pose_arm_default'}, transitions={'picked':'picked', 'pick_failed':'pick_failed', 'preempted':'preempted', 'error':'error'}) asw = ActionServerWrapper('pick_object', pick_and_place_msgs.PickObjectAction, wrapped_container = sm, goal_key = 'goal', feedback_key = 'feedback', result_key = 'result', succeeded_outcomes = ['picked'], aborted_outcomes = ['pick_failed','error'], preempted_outcomes = ['preempted']) asw.run_server() rospy.spin() return
def main(): rospy.init_node('place_object') sm = smach.StateMachine( outcomes=['placed', 'place_failed', 'preempted', 'error'], input_keys=['goal'], output_keys=['feedback', 'result']) with sm: sm.userdata.result = pick_and_place_msgs.PickObjectResult() smach.StateMachine.add('ParseGoal', ParseGoal(), remapping={ 'goal': 'goal', 'feedback': 'feedback', 'place_pose': 'place_pose' }, transitions={'parsed': 'PlaceObject'}) sm_place = place_object_sm.createSM() smach.StateMachine.add('PlaceObject', sm_place, remapping={ 'object_name': 'object_name', 'place_pose': 'place_pose' }, transitions={ 'placed': 'placed', 'place_failed': 'place_failed', 'preempted': 'preempted', 'error': 'error' }) asw = ActionServerWrapper('place_object', pick_and_place_msgs.PlaceObjectAction, wrapped_container=sm, goal_key='goal', feedback_key='feedback', result_key='result', succeeded_outcomes=['placed'], aborted_outcomes=['place_failed', 'error'], preempted_outcomes=['preempted']) asw.run_server() rospy.spin() return
def main(): sm_top = StateMachine( outcomes = ['success', 'localize_failed', 'navi_failed', 'get_failed', 'preempted'], input_keys = ['goal_message', 'result_message'], output_keys = ['result_message']) with sm_top: StateMachine.add( 'LOCALIZATION', Localization(), transitions = {'localize':'GET_COORDINATE', 'not_localize':'localize_failed'}, remapping = {'goal_in':'goal_message'}) StateMachine.add( 'GET_COORDINATE', GetCootdinate(), transitions = {'get':'NAVIGATION', 'not_get':'get_failed'}, remapping = {'coord_out':'person_coord'}) StateMachine.add( 'NAVIGATION', Navigation(), transitions = {'arrive':'success', 'not_arrive':'navi_failed'}, remapping = {'result_message':'result_message', 'coord_in':'person_coord'}) asw = ActionServerWrapper( 'approach_person', ApproachPersonAction, wrapped_container = sm_top, succeeded_outcomes = ['success'], aborted_outcomes = ['find_failed', 'get_failed', 'navi_failed'], preempted_outcomes = ['preempted'], goal_key = 'goal_message', result_key = 'result_message') asw.run_server() rospy.spin()
def test_action_server_wrapper(self): """Test action server wrapper.""" sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded') sq.register_input_keys(['goal', 'action_goal', 'action_result']) sq.register_output_keys(['action_result']) with sq: Sequence.add( 'GOAL_KEY', SimpleActionState("reference_action", TestAction, goal_key='action_goal')) Sequence.add( 'GOAL_SLOTS', SimpleActionState("reference_action", TestAction, goal_slots=['goal'])) @cb_interface(input_keys=['action_result'], output_keys=['action_result']) def res_cb(ud, status, res): ud.action_result.result = res.result + 1 Sequence.add( 'RESULT_CB', SimpleActionState("reference_action", TestAction, goal=g1, result_cb=res_cb)) asw = ActionServerWrapper('reference_action_sm', TestAction, sq, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted'], preempted_outcomes=['preempted'], expand_goal_slots=True) asw.run_server() ac = SimpleActionClient('reference_action_sm', TestAction) ac.wait_for_server(rospy.Duration(30)) assert ac.send_goal_and_wait( g1, rospy.Duration(30)) == GoalStatus.SUCCEEDED assert ac.get_result().result == 1
def __init__(self, server_name, wrapped_container, succeeded_outcomes=["succeeded"], aborted_outcomes=["aborted"], preempted_outcomes=["preempted"], goal_key='action_goal', feedback_key='action_feedback', result_key='action_result', goal_slots_map={}, feedback_slots_map={}, result_slots_map={}, expand_goal_slots=False, pack_result_slots=False): ActionServerWrapper.__init__( self, server_name, ExecuteSmachStateMachineAction, wrapped_container, succeeded_outcomes, aborted_outcomes, preempted_outcomes, goal_key, feedback_key, result_key, goal_slots_map, feedback_slots_map, result_slots_map, expand_goal_slots, pack_result_slots)
def test_action_preemption(self): """Test action preemption""" sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded') class SlowRunningState(State): def __init__(self): State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted']) def execute(self, ud): start_time = rospy.Time.now() while rospy.Time.now() - start_time < rospy.Duration(10): rospy.sleep(0.05) if self.preempt_requested(): self.service_preempt() return 'preempted' return 'succeeded' with sq: Sequence.add('PREEMPT_ME', SlowRunningState()) asw = ActionServerWrapper('preempt_action_sm', TestAction, sq, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted'], preempted_outcomes=['preempted']) asw.run_server() ac = SimpleActionClient('preempt_action_sm', TestAction) ac.wait_for_server(rospy.Duration(30)) ac.send_goal(g1) rospy.sleep(5.0) ac.cancel_goal() start_time = rospy.Time.now() while ac.get_state() == GoalStatus.ACTIVE and rospy.Time.now( ) - start_time < rospy.Duration(30): rospy.sleep(0.5) assert ac.get_state() == GoalStatus.PREEMPTED
def main(mokeup=False): # Open the container rospy.init_node('insert_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['insert_object_goal'], output_keys = ['insert_object_feedback', 'insert_object_result']) with sm: if not mokeup: smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'), transitions={'success':'GENERATE_OBJECT_POSE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'), transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'}) # waits for object selector response, if failure this means that the string published previously was not found in object list smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0), transitions={'success':'PLAN_WHOLE_BODY_MOTION', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_start'), transitions={'success':'WAIT_PLAN_WHOLE_BODY_MOTION'}) # wait for the result of the pregrasp planner smach.StateMachine.add('WAIT_PLAN_WHOLE_BODY_MOTION', wait_for_event('/whole_body_motion_calculator_pipeline/event_out', 15.0), transitions={'success':'CALCULATE_BASE_MOTION', 'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', 'failure':'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'}) # pregrasp planner failed or timeout, stop the component and then return overall failure smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'), transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion smach.StateMachine.add('CALCULATE_BASE_MOTION', gbs.send_and_wait_events_combined( event_in_list=[('/base_motion_calculator/event_in','e_start')], event_out_list=[('/base_motion_calculator/event_out','e_success', True)], timeout_duration=5), transitions={'success':'STOP_PLAN_WHOLE_BODY_MOTION', 'timeout':'STOP_MOVE_BASE_TO_OBJECT', 'failure':'STOP_MOVE_BASE_TO_OBJECT'}) # wbc pipeline was successful, so lets stop it since its work is done smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'), transitions={'success':'UNSTAGE_OBJECT'}) smach.StateMachine.add('UNSTAGE_OBJECT', unstage(), transitions={'succeeded': 'MOVE_ROBOT_TO_OBJECT', 'failed' : 'SET_ACTION_LIB_FAILURE'}) # execute robot motion smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')], event_out_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)], timeout_duration=5), transitions={'success':'STOP_MOVE_BASE_TO_OBJECT', 'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'}) # send stop event_in to arm motion component smach.StateMachine.add('STOP_MOVE_BASE_TO_OBJECT', send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'), transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # send stop event_in to arm motion component and return failure smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'), transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) ########################################## PREGRASP PIPELINE 2 ################################################## # based on a published pose, calls pregrasp planner to generate a graspable pose smach.StateMachine.add('SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/pregrasp_planner_pipeline/event_in','e_start'), transitions={'success':'WAIT_PLAN_ARM_MOTION_2'}) # wait for the result of the pregrasp planner smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2', wait_for_event('/pregrasp_planner_pipeline/event_out', 15.0), transitions={'success':'STOP_PLAN_ARM_MOTION_2', 'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', 'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'}) smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event('/pregrasp_planner_pipeline/event_in','e_stop'), transitions={'success':'MOVE_ARM_TO_OBJECT_2'}) # execute robot motion smach.StateMachine.add('MOVE_ARM_TO_OBJECT_2', gbs.send_and_wait_events_combined( event_in_list=[('/waypoint_trajectory_generation/event_in','e_start')], event_out_list=[('/waypoint_trajectory_generation/event_out','e_success', True)], timeout_duration=10), transitions={'success':'STOP_MOVE_ARM_TO_OBJECT_2', 'timeout':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', 'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'), transitions={'success':'OPEN_GRIPPER'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'), transitions={'success':'MOVE_ARM_TO_DEFAULT_PLACE'}) # close gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'), transitions={'succeeded': 'MOVE_ARM_TO_HOLD'}) # move arm to HOLD position smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={'succeeded':'SET_ACTION_LIB_SUCCESS', 'failed':'MOVE_ARM_TO_HOLD'}) # place in case of failure smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("10cm/pose4"), transitions={'succeeded':'OPEN_GRIPPER_DEFAULT', 'failed':'MOVE_ARM_TO_DEFAULT_PLACE'}) smach.StateMachine.add('OPEN_GRIPPER_DEFAULT', gms.control_gripper('open_narrow'), transitions={'succeeded': 'MOVE_ARM_BACK'}) smach.StateMachine.add('MOVE_ARM_BACK', gms.move_arm("look_at_turntable"), transitions={'succeeded':'SET_ACTION_LIB_SUCCESS', 'failed':'SET_ACTION_LIB_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded':'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) """smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"), transitions={'succeeded':'OVERALL_FAILED', 'failed':'OVERALL_FAILED'})""" # smach viewer sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'insert_object_server', action_spec = InsertObjectAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'insert_object_goal', feedback_key = 'insert_object_feedback', result_key = 'insert_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): # Open the container rospy.init_node("insert_object_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) with sm: smach.StateMachine.add( "SELECT_OBJECT", SelectObject("/mcr_perception/object_selector/input/object_name"), transitions={"succeeded": "GENERATE_OBJECT_POSE"}, ) # generates a pose of object smach.StateMachine.add( "GENERATE_OBJECT_POSE", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_selector/event_in", "e_trigger")], event_out_list=[( "/mcr_perception/object_selector/event_out", "e_selected", True, )], timeout_duration=10, ), transitions={ "success": "SET_DBC_PARAMS", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "SET_DBC_PARAMS", gbs.set_named_config("dbc_pick_object"), transitions={ "success": "MOVE_ROBOT_AND_TRY_PLACING", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "MOVE_ROBOT_AND_TRY_PLACING", gbs.send_and_wait_events_combined( event_in_list=[("/wbc/event_in", "e_try")], event_out_list=[("/wbc/event_out", "e_success", True)], timeout_duration=50, ), transitions={ "success": "UNSTAGE_OBJECT", "timeout": "UNSTAGE_OBJECT", "failure": "UNSTAGE_OBJECT", }, ) smach.StateMachine.add( "UNSTAGE_OBJECT", gas.unstage_object(), transitions={ "success": "RE_GENERATE_OBJECT_POSE", "failed": "OVERALL_FAILED", }, ) # generates a pose based on the previous string object topic received smach.StateMachine.add( "RE_GENERATE_OBJECT_POSE", gbs.send_event([("/mcr_perception/object_selector/event_in", "e_re_trigger")]), transitions={"success": "MOVE_ROBOT_AND_PLACE"}, ) # execute robot motion smach.StateMachine.add( "MOVE_ROBOT_AND_PLACE", gbs.send_and_wait_events_combined( event_in_list=[("/wbc/event_in", "e_start")], event_out_list=[("/wbc/event_out", "e_success", True)], timeout_duration=20, ), transitions={ "success": "OPEN_GRIPPER", "timeout": "MOVE_ARM_TO_DEFAULT_PLACE", "failure": "MOVE_ARM_TO_DEFAULT_PLACE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_DEFAULT_PLACE", gms.move_arm("look_at_turntable"), transitions={ "succeeded": "MOVE_ARM_CARTESIAN", "failed": "MOVE_ARM_TO_DEFAULT_PLACE", }, ) smach.StateMachine.add( "MOVE_ARM_CARTESIAN", gbs.send_and_wait_events_combined( event_in_list=[("/insert_object_workaroud/event_in", "e_start") ], event_out_list=[("/insert_object_workaroud/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "OPEN_GRIPPER", "timeout": "STOP_ARM_CARTESIAN_MOTION", "failure": "OPEN_GRIPPER", }, ) smach.StateMachine.add( "STOP_ARM_CARTESIAN_MOTION", gbs.send_event([("/insert_object_workaroud/event_in", "e_stop")]), transitions={"success": "OPEN_GRIPPER"}, ) smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open_narrow"), transitions={"succeeded": "MOVE_ARM_TO_HOLD"}, ) # move arm to HOLD position smach.StateMachine.add( "MOVE_ARM_TO_HOLD", gms.move_arm("look_at_turntable"), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "MOVE_ARM_TO_HOLD", }, ) sm.register_transition_cb(transition_cb) sm.register_start_cb(start_cb) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = smach_ros.IntrospectionServer("insert_object_smach_viewer", sm, "/INSERT_OBJECT_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="insert_object_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('place_object_server') # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['goal', 'feedback'], output_keys=['feedback', 'result']) with sm: #add states to the container smach.StateMachine.add('CHECK_IF_SHELF_INITIAL', CheckIfLocationIsShelf(), transitions={ 'shelf': 'MOVE_ARM_TO_PRE_PLACE_SHELF', 'not_shelf': 'MOVE_ARM_TO_PRE_PLACE' }) smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE_SHELF', gms.move_arm("pre_place_shelf"), transitions={ 'succeeded': 'START_PLACE_POSE_SELECTOR', 'failed': 'MOVE_ARM_TO_PRE_PLACE_SHELF' }) smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'START_PLACE_POSE_SELECTOR', 'failed': 'MOVE_ARM_TO_PRE_PLACE' }) smach.StateMachine.add( 'START_PLACE_POSE_SELECTOR', gbs.send_event([('/mcr_perception/place_pose_selector/event_in', 'e_start')]), transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'}) smach.StateMachine.add( 'GET_POSE_TO_PLACE_OBJECT', GetPoseToPlaceOject( '/mcr_perception/place_pose_selector/platform_name', '/mcr_perception/place_pose_selector/place_pose', '/mcr_perception/place_pose_selector/event_out', 15.0), transitions={ 'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT', 'failed': 'MOVE_ARM_TO_DEFAULT_PLACE' }) smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("15cm/pose4"), transitions={ 'succeeded': 'OPEN_GRIPPER', 'failed': 'MOVE_ARM_TO_DEFAULT_PLACE' }) smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT', gms.move_arm(), transitions={ 'succeeded': 'OPEN_GRIPPER', 'failed': 'OPEN_GRIPPER' }) smach.StateMachine.add( 'OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'}) smach.StateMachine.add( 'STOP_PLACE_POSE_SELECTOR', gbs.send_event([('/mcr_perception/place_pose_selector/event_in', 'e_stop')]), transitions={'success': 'CHECK_IF_SHELF'}) smach.StateMachine.add('CHECK_IF_SHELF', CheckIfLocationIsShelf(), transitions={ 'shelf': 'MOVE_ARM_TO_SAFE_1', 'not_shelf': 'MOVE_ARM_TO_NEUTRAL' }) smach.StateMachine.add('MOVE_ARM_TO_NEUTRAL', gms.move_arm("barrier_tape"), transitions={ 'succeeded': 'OVERALL_SUCCESS', 'failed': 'MOVE_ARM_TO_NEUTRAL' }) smach.StateMachine.add('MOVE_ARM_TO_SAFE_1', gms.move_arm("shelf_intermediate_2"), transitions={ 'succeeded': 'MOVE_ARM_TO_SAFE_2', 'failed': 'MOVE_ARM_TO_SAFE_1' }) smach.StateMachine.add('MOVE_ARM_TO_SAFE_2', gms.move_arm("pre_place_shelf"), transitions={ 'succeeded': 'MOVE_ARM_TO_SAFE_3', 'failed': 'MOVE_ARM_TO_SAFE_2' }) smach.StateMachine.add('MOVE_ARM_TO_SAFE_3', gms.move_arm("platform_left_pre"), transitions={ 'succeeded': 'OVERALL_SUCCESS', 'failed': 'MOVE_ARM_TO_SAFE_3' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = IntrospectionServer('place_object_smach_viewer', sm, '/STAGE_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='place_object_server', action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='goal', feedback_key='feedback', result_key='result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('compute_base_shift_server') # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['compute_base_shift_goal'], output_keys=[ 'compute_base_shift_feedback', 'compute_base_shift_result' ]) sm.userdata.next_arm_pose_index = 0 sm.userdata.move_arm_to = None with sm: #add states to the container # remove accumulated base poses and start non loop dependant components smach.StateMachine.add( 'CLEAR_BASE_POSES', gbs.send_event([ ('/move_base_relative/pose_selector/event_in', 'e_forget'), ('/move_base_relative/random_obj_selector_transformer_republisher/event_in', 'e_start'), ('/move_base_relative/modified_pose_transformer/event_in', 'e_start') ]), transitions={'success': 'SELECT_NEXT_LOOK_POSE'}) # select a look_at_workspace pose smach.StateMachine.add( 'SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace']), transitions={ 'succeeded': 'LOOK_AROUND', # next pose selected 'completed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS', 'failed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS' } ) # we've run out of poses to select, which means we've gone through the list # move arm to selected pose smach.StateMachine.add('LOOK_AROUND', gms.move_arm(), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS', 'failed': 'LOOK_AROUND' }) smach.StateMachine.add( 'RECOGNIZE_OBJECTS', gps.find_objects(retries=1), transitions={ 'objects_found': 'SELECT_OBJECT', 'no_objects_found': 'SELECT_NEXT_LOOK_POSE' }, remapping={'found_objects': 'recognized_objects'}) # here the object selector is set up to select objects randomly # it will publish object name (goes to pose selector) and object pose (goes to compute base shift pipeline) # relative displacement calculator pose republisher in odom frame is also triggered here, before doing computations smach.StateMachine.add( 'SELECT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/random_object_selector/event_in', 'e_trigger') ], event_out_list=[ ('/mcr_perception/random_object_selector/event_out', 'e_selected', True) ], timeout_duration=10), transitions={ 'success': 'SELECT_OBJECT', 'timeout': 'SELECT_NEXT_LOOK_POSE', 'failure': 'SELECT_NEXT_LOOK_POSE' } ) # this means we're done computing poses for all recognized objects from this pose # stop relative displacement calculator pose republisher and after that go to success smach.StateMachine.add( 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS', gbs.send_event([ ('/move_base_relative/random_obj_selector_transformer_republisher/event_in', 'e_stop'), ('/move_base_relative/modified_pose_transformer/event_in', 'e_stop') ]), transitions={'success': 'SET_ACTION_LIB_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper(server_name='compute_base_shift_server', action_spec=ComputeBaseShiftAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='compute_base_shift_goal', feedback_key='compute_base_shift_feedback', result_key='compute_base_shift_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('perceive_cavity_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['perceive_cavity_goal'], output_keys=['perceive_cavity_feedback', 'perceive_cavity_result']) sm.userdata.next_arm_pose_index = 0 # Open the container with sm: # approach to platform smach.StateMachine.add( 'GET_GOAL', getGoal(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME_FOR_WBC'}) # generates a pose based on the previous string object topic received smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME_FOR_WBC', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'START_POSE_SELECTOR'}) # start the cavity pose selector to accumulate the poses smach.StateMachine.add( 'START_POSE_SELECTOR', gbs.send_event([('/mcr_perception/cavity_pose_selector/event_in', 'e_start')]), transitions={'success': 'SELECT_NEXT_LOOK_POSE'}) # select a look_at_workspace pose smach.StateMachine.add( 'SELECT_NEXT_LOOK_POSE', gms.select_arm_pose([ 'look_straight_at_workspace_left', 'look_straight_at_workspace_right' ]), transitions={ 'succeeded': 'LOOK_AROUND', # next pose selected 'completed': 'SET_ACTION_LIB_SUCCESS', # we've run out of poses to select, which means we've gone through the list 'failed': 'SET_ACTION_LIB_SUCCESS' } ) # we've run out of poses to select, which means we've gone through the list # move arm to selected pose smach.StateMachine.add('LOOK_AROUND', gms.move_arm(), transitions={ 'succeeded': 'RECOGNIZE_CAVITIES', 'failed': 'LOOK_AROUND' }) # trigger perception pipeline smach.StateMachine.add('RECOGNIZE_CAVITIES', gps.find_cavities(retries=1), transitions={ 'cavities_found': 'SELECT_NEXT_LOOK_POSE', 'no_cavities_found': 'SELECT_NEXT_LOOK_POSE' }) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('perceive_cavity_smach_viewer', sm, '/PERCEIVE_CAVITY_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_cavity_server', action_spec=PerceiveLocationAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='perceive_cavity_goal', feedback_key='perceive_cavity_feedback', result_key='perceive_cavity_result') # Run the server in a background thread asw.run_server() rospy.spin()
NewGoalState(ws), transitions={'succeeded': 'MOVE_TO'}) smach.StateMachine.add('MOVE_TO', SimpleActionState('/motion_controller/move_to', MoveToAction, goal_key='action_goal'), transitions={'preempted': 'aborted', 'aborted': 'WALLFOLLOWER'}) smach.StateMachine.add('WALLFOLLOWER', ws, transitions={'leave_wall': 'MOVE_TO'}) # Wrap an action server around the state machine. asw = ActionServerWrapper('~move_to', MoveToAction, wrapped_container=sm, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted, unreachable_goal'], preempted_outcomes=['preempted']) # Similarly to how it is done in MotionController, create a combination of # listened and re-publisher to allow the user to send a goal as a message # (not as an action service request). action_goal_pub = rospy.Publisher('~move_to/goal', MoveToActionGoal) def simple_goal_cb(target_pose): rospy.loginfo('Received target pose through the "simple goal" topic. ' 'Wrapping it in the action message and forwarding to ' 'the server.') msg = MoveToActionGoal() msg.header.stamp = rospy.Time.now() msg.goal.target_pose = target_pose
def main(): rospy.init_node('insert_cavity_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['goal'], output_keys = ['feedback', 'result']) with sm: # publish object as string to mcr_perception_selectors -> cavity, # this component then publishes pose in base_link reference frame smach.StateMachine.add('SELECT_OBJECT', SelectObject( '/mcr_perception/cavity_pose_selector/object_name'), transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE'}) smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')], event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)], timeout_duration=10), transitions={'success':'SET_DBC_PARAMS', 'timeout':'UNSTAGE_OBJECT', 'failure':'UNSTAGE_OBJECT'}) smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'), transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING', 'timeout':'OVERALL_FAILED', 'failure':'OVERALL_FAILED'}) smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined( event_in_list=[('/wbc/event_in','e_try')], event_out_list=[('/wbc/event_out','e_success', True)], timeout_duration=50), transitions={'success':'STOP_POSE_SELECTOR', 'timeout':'STOP_POSE_SELECTOR', 'failure':'STOP_POSE_SELECTOR'}) smach.StateMachine.add('STOP_POSE_SELECTOR', gbs.send_event( [('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]), transitions={'success':'PERCEIVE_CAVITY'}) # perceive cavity again after moving in front of the desired cavity smach.StateMachine.add('PERCEIVE_CAVITY', gas.perceive_cavity(), transitions={'success': 'SELECT_OBJECT_AGAIN', 'failed' : 'OVERALL_FAILED'}) smach.StateMachine.add('SELECT_OBJECT_AGAIN', SelectObject( '/mcr_perception/cavity_pose_selector/object_name'), transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN'}) smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')], event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)], timeout_duration=10), transitions={'success':'UNSTAGE_OBJECT', 'timeout':'OVERALL_FAILED', 'failure':'OVERALL_FAILED'}) smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(), transitions={'success': 'TRY_INSERTING', 'failed' : 'OVERALL_FAILED'}) # execute robot motion smach.StateMachine.add('TRY_INSERTING', gbs.send_and_wait_events_combined( event_in_list=[('/wbc/event_in','e_start_arm_only')], event_out_list=[('/wbc/event_out','e_success', True)], timeout_duration=20), transitions={'success':'OPEN_GRIPPER', 'timeout':'STAGE_OBJECT', 'failure':'STAGE_OBJECT'}) smach.StateMachine.add('STAGE_OBJECT', gas.stage_object(), transitions={'success': 'OVERALL_FAILED', 'failed' : 'OVERALL_FAILED'}) # close gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'WIGGLE_ARM'}) # wiggling the arm for precision placement smach.StateMachine.add('WIGGLE_ARM', ppt_wiggle_arm(wiggle_offset=-0.12, joint=0), transitions={'succeeded':'MOVE_ARM_TO_HOLD', 'failed':'MOVE_ARM_TO_HOLD'}) # move arm to HOLD position smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={'succeeded':'STOP_POSE_SELECTOR_FINAL', 'failed':'MOVE_ARM_TO_HOLD'}) # sending e_stop to pose selector smach.StateMachine.add('STOP_POSE_SELECTOR_FINAL', gbs.send_event( [('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]), transitions={'success':'OVERALL_SUCCESS'}) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = IntrospectionServer('insert_cavity_smach_viewer', sm, '/INSERT_CAVITY_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'insert_cavity_server', action_spec = GenericExecuteAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'goal', feedback_key = 'feedback', result_key = 'result') # Run the server in a background thread asw.run_server() rospy.spin()
smach.StateMachine.add('MOVE_TO', SimpleActionState('/motion_controller/move_to', MoveToAction, goal_key='action_goal'), transitions={ 'preempted': 'aborted', 'aborted': 'WALLFOLLOWER' }) smach.StateMachine.add('WALLFOLLOWER', ws, transitions={'leave_wall': 'MOVE_TO'}) # Wrap an action server around the state machine. asw = ActionServerWrapper('~move_to', MoveToAction, wrapped_container=sm, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted, unreachable_goal'], preempted_outcomes=['preempted']) # Similarly to how it is done in MotionController, create a combination of # listened and re-publisher to allow the user to send a goal as a message # (not as an action service request). action_goal_pub = rospy.Publisher('~move_to/goal', MoveToActionGoal, queue_size=100) def simple_goal_cb(target_pose): rospy.loginfo('Received target pose through the "simple goal" topic. ' 'Wrapping it in the action message and forwarding to ' 'the server.') msg = MoveToActionGoal()
def main(): rospy.init_node('smach_example_state_machine') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['insert_object_goal'], output_keys=['insert_object_feedback', 'insert_object_result']) with sm: # publish object as string to mcr_perception_selectors -> object_selector, this component then publishes # pose in base_link reference frame when e_trigger is sent (next state) smach.StateMachine.add( 'SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'), transitions={'success': 'GENERATE_OBJECT_POSE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event( '/mcr_perception/object_selector/event_in', 'e_trigger'), transitions={'success': 'SET_TOP_HOLE_CONFIG'}) # reconfigure top hole shifter smach.StateMachine.add('SET_TOP_HOLE_CONFIG', gbs.set_named_config('top_hole_shifter'), transitions={ 'success': 'GENERATE_TOP_HOLE_POSE', 'failure': 'SET_ACTION_LIB_FAILURE', 'timeout': 'SET_TOP_HOLE_CONFIG' }) # receive a pose and offset's up, result: top hole pose (another pose) smach.StateMachine.add( 'GENERATE_TOP_HOLE_POSE', send_event('/top_hole_pose_shifter/event_in', 'e_start'), transitions={'success': 'WAIT_FOR_CREATE_TOP_HOLE_POSE'}) # wait for pose shifter to give response smach.StateMachine.add('WAIT_FOR_CREATE_TOP_HOLE_POSE', wait_for_event( '/top_hole_pose_shifter/event_out', timeout_duration=5.0), transitions={ 'success': 'SET_TOP_HOLE_PREGRASP_CONFIG', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # reconfigure top hole pregrasp params smach.StateMachine.add('SET_TOP_HOLE_PREGRASP_CONFIG', gbs.set_named_config('top_hole_pregrasp'), transitions={ 'success': 'PLAN_ARM_MOTION', 'failure': 'SET_ACTION_LIB_FAILURE', 'timeout': 'SET_TOP_HOLE_PREGRASP_CONFIG' }) # based on a published pose, calls pregrasp planner to generate a graspable pose smach.StateMachine.add( 'PLAN_ARM_MOTION', send_event('/top_hole_pregrasp_planner_pipeline/event_in', 'e_start'), transitions={'success': 'WAIT_PLAN_ARM_MOTION'}) # wait for the result of the pregrasp planner smach.StateMachine.add( 'WAIT_PLAN_ARM_MOTION', wait_for_event('/top_hole_pregrasp_planner_pipeline/event_out', 3.0), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', 'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE', 'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE' }) # pregrasp planner was successful, so lets stop it since its work is done smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION', send_event('/top_hole_pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) # pregrasp planner failed or timeout, stop the component and then return overall failure smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION_WITH_FAILURE', send_event('/top_hole_pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) # move arm to pregrasp planned pose smach.StateMachine.add( 'MOVE_ARM_TO_OBJECT', send_event('/top_hole_move_arm_planned/event_in', 'e_start'), transitions={'success': 'WAIT_MOVE_ARM_TO_OBJECT'}) # wait for the arm motion to finish smach.StateMachine.add('WAIT_MOVE_ARM_TO_OBJECT', wait_for_event( '/top_hole_move_arm_planned/event_out', 10.0), transitions={ 'success': 'STOP_MOVE_ARM_TO_OBJECT', 'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', 'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE' }) # send stop event_in to arm motion component and return failure smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', send_event('/top_hole_move_arm_planned/event_in', 'e_stop'), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) # send stop event_in to arm motion component smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT', send_event('/top_hole_move_arm_planned/event_in', 'e_stop'), transitions={'success': 'CONFIGURE_POSES_TO_MOVE_DOWN'}) # reconfigure the pose smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_DOWN', gbs.set_named_config('move_down_shifter'), transitions={ 'success': 'PLAN_LINEAR_MOTION_DOWN', 'failure': 'SET_ACTION_LIB_FAILURE', 'timeout': 'CONFIGURE_POSES_TO_MOVE_DOWN' }) # linear motion down states smach.StateMachine.add( 'PLAN_LINEAR_MOTION_DOWN', send_event('/poses_to_move/event_in', 'e_start'), transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_DOWN'}) smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_DOWN', wait_for_event('/poses_to_move/event_out', timeout_duration=5.0), transitions={ 'success': 'MOVE_TO_CARTESIAN_POSE_DOWN', 'failure': 'MOVE_ARM_TO_HOLD_FAILED', 'timeout': 'MOVE_ARM_TO_HOLD_FAILED' }) # linear motion common states smach.StateMachine.add( 'MOVE_TO_CARTESIAN_POSE_DOWN', send_event('/cartesian_controller_demo/event_in', 'e_start'), transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN'}) smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN', wait_for_event( '/cartesian_controller_demo/event_out', timeout_duration=5.0), transitions={ 'success': 'OPEN_GRIPPER', 'timeout': 'MOVE_ARM_TO_HOLD_FAILED', 'failure': 'MOVE_ARM_TO_HOLD_FAILED' }) # open gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'OPEN_GRIPPER_AGAIN'}) # open gripper again (replace later on with padmaja stuff) smach.StateMachine.add( 'OPEN_GRIPPER_AGAIN', gms.control_gripper('open'), transitions={'succeeded': 'CONFIGURE_POSES_TO_MOVE_UP'}) #reconfigure smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_UP', gbs.set_named_config('move_up_shifter'), transitions={ 'success': 'PLAN_LINEAR_MOTION_UP', 'failure': 'SET_ACTION_LIB_FAILURE', 'timeout': 'CONFIGURE_POSES_TO_MOVE_UP' }) # linear motion up states smach.StateMachine.add( 'PLAN_LINEAR_MOTION_UP', send_event('/poses_to_move/event_in', 'e_start'), transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_UP'}) smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_UP', wait_for_event('/poses_to_move/event_out', timeout_duration=5.0), transitions={ 'success': 'MOVE_TO_CARTESIAN_POSE_UP', 'timeout': 'MOVE_ARM_TO_HOLD', 'failure': 'MOVE_ARM_TO_HOLD' }) # linear motion common states smach.StateMachine.add( 'MOVE_TO_CARTESIAN_POSE_UP', send_event('/cartesian_controller_demo/event_in', 'e_start'), transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP'}) smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP', wait_for_event( '/cartesian_controller_demo/event_out', timeout_duration=5.0), transitions={ 'success': 'MOVE_ARM_TO_HOLD', 'timeout': 'MOVE_ARM_TO_HOLD', 'failure': 'MOVE_ARM_TO_HOLD' }) # move arm to HOLD smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'MOVE_ARM_TO_HOLD' }) # move arm to HOLD but provided that some old component failed smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILED', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'SET_ACTION_LIB_FAILURE', 'failed': 'MOVE_ARM_TO_HOLD_FAILED' }) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='insert_object_server', action_spec=InsertObjectAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='insert_object_goal', feedback_key='insert_object_feedback', result_key='insert_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node("place_object_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal", "feedback"], output_keys=["feedback", "result"], ) with sm: # add states to the container smach.StateMachine.add( "CHECK_IF_SHELF_INITIAL", CheckIfLocationIsShelf(), transitions={ "shelf": "MOVE_ARM_TO_PRE_PLACE_SHELF", "not_shelf": "MOVE_ARM_TO_PRE_PLACE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_PRE_PLACE_SHELF", gms.move_arm("pre_place_shelf"), transitions={ "succeeded": "START_PLACE_POSE_SELECTOR", "failed": "MOVE_ARM_TO_PRE_PLACE_SHELF", }, ) smach.StateMachine.add( "MOVE_ARM_TO_PRE_PLACE", gms.move_arm("look_at_turntable"), transitions={ "succeeded": "START_PLACE_POSE_SELECTOR", "failed": "MOVE_ARM_TO_PRE_PLACE", }, ) smach.StateMachine.add( "START_PLACE_POSE_SELECTOR", gbs.send_event([("/mcr_perception/place_pose_selector/event_in", "e_start")]), transitions={"success": "GET_POSE_TO_PLACE_OBJECT"}, ) smach.StateMachine.add( "GET_POSE_TO_PLACE_OBJECT", GetPoseToPlaceOject( "/mcr_perception/place_pose_selector/platform_name", "/mcr_perception/place_pose_selector/place_pose", "/mcr_perception/place_pose_selector/event_out", 15.0, ), transitions={ "succeeded": "MOVE_ARM_TO_PLACE_OBJECT", "failed": "MOVE_ARM_TO_DEFAULT_PLACE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_DEFAULT_PLACE", gms.move_arm("15cm/pose4"), transitions={ "succeeded": "OPEN_GRIPPER", "failed": "MOVE_ARM_TO_DEFAULT_PLACE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_PLACE_OBJECT", gms.move_arm(), transitions={ "succeeded": "OPEN_GRIPPER", "failed": "OPEN_GRIPPER", }, ) smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "STOP_PLACE_POSE_SELECTOR"}, ) smach.StateMachine.add( "STOP_PLACE_POSE_SELECTOR", gbs.send_event([("/mcr_perception/place_pose_selector/event_in", "e_stop")]), transitions={"success": "CHECK_IF_SHELF"}, ) smach.StateMachine.add( "CHECK_IF_SHELF", CheckIfLocationIsShelf(), transitions={ "shelf": "MOVE_ARM_TO_SAFE_1", "not_shelf": "MOVE_ARM_TO_NEUTRAL", }, ) smach.StateMachine.add( "MOVE_ARM_TO_NEUTRAL", gms.move_arm("barrier_tape"), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "MOVE_ARM_TO_NEUTRAL", }, ) smach.StateMachine.add( "MOVE_ARM_TO_SAFE_1", gms.move_arm("shelf_intermediate_2"), transitions={ "succeeded": "MOVE_ARM_TO_SAFE_2", "failed": "MOVE_ARM_TO_SAFE_1", }, ) smach.StateMachine.add( "MOVE_ARM_TO_SAFE_2", gms.move_arm("pre_place_shelf"), transitions={ "succeeded": "MOVE_ARM_TO_SAFE_3", "failed": "MOVE_ARM_TO_SAFE_2", }, ) smach.StateMachine.add( "MOVE_ARM_TO_SAFE_3", gms.move_arm("platform_left_pre"), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "MOVE_ARM_TO_SAFE_3", }, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = IntrospectionServer("place_object_smach_viewer", sm, "/STAGE_OBJECT_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="place_object_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('place_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['place_object_goal', 'place_object_feedback'], output_keys=['place_object_feedback', 'place_object_result']) with sm: #add states to the container smach.StateMachine.add( 'START_PLACE_POSE_SELECTOR', gbs.send_event([('/mcr_perception/place_pose_selector/event_in', 'e_start')]), transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'}) smach.StateMachine.add( 'GET_POSE_TO_PLACE_OBJECT', GetPoseToPlaceOject( '/mcr_perception/place_pose_selector/platform_name', '/mcr_perception/place_pose_selector/place_pose', '/mcr_perception/place_pose_selector/event_out', 15.0), transitions={ 'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT', 'failed': 'SET_ACTION_LIB_FAILURE' }) smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT', gms.move_arm(), transitions={ 'succeeded': 'OPEN_GRIPPER', 'failed': 'OPEN_GRIPPER' }) smach.StateMachine.add( 'OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'}) """smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("navigation"), transitions={'succeeded':'STOP_PLACE_POSE_SELECTOR', 'failed':'MOVE_ARM_TO_HOLD'})""" smach.StateMachine.add( 'STOP_PLACE_POSE_SELECTOR', gbs.send_event([('/mcr_perception/place_pose_selector/event_in', 'e_stop')]), transitions={'success': 'SET_ACTION_LIB_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('place_object_smach_viewer', sm, '/STAGE_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='place_object_server', action_spec=PlaceObjectAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='place_object_goal', feedback_key='place_object_feedback', result_key='place_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('unstage_object_server') # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['goal'], output_keys=['feedback', 'result']) with sm: #add states to the container smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE', gms.move_arm('stage_intermediate'), transitions={ 'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2', 'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE' }) smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2', gms.move_arm('stage_intermediate_2'), transitions={ 'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE', 'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2' }) smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE', SetupMoveArm('pre'), transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'}) smach.StateMachine.add('MOVE_ARM_PRE_STAGE', gms.move_arm_and_gripper('open_narrow'), transitions={ 'succeeded': 'SETUP_MOVE_ARM_STAGE', 'failed': 'MOVE_ARM_PRE_STAGE' }) smach.StateMachine.add('SETUP_MOVE_ARM_STAGE', SetupMoveArm('final'), transitions={'succeeded': 'MOVE_ARM_STAGE'}) smach.StateMachine.add('MOVE_ARM_STAGE', gms.move_arm(), transitions={ 'succeeded': 'CLOSE_GRIPPER', 'failed': 'MOVE_ARM_STAGE' }) smach.StateMachine.add( 'CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'}) #TODO: verify if object is grasped or not smach.StateMachine.add( 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN', SetupMoveArm('pre'), transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN'}) smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN', gms.move_arm(), transitions={ 'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL', 'failed': 'MOVE_ARM_PRE_STAGE_AGAIN' }) smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL', gms.move_arm('stage_intermediate_2'), transitions={ 'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL', 'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL' }) smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL', gms.move_arm('stage_intermediate'), transitions={ 'succeeded': 'OVERALL_SUCCESS', 'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm, '/UNSTAGE_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='unstage_object_server', action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='goal', feedback_key='feedback', result_key='result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(mokeup=False): # Open the container rospy.init_node('pick_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['pick_object_goal'], output_keys=['pick_object_feedback', 'pick_object_result']) with sm: if not mokeup: # publish object as string to mcr_perception_selectors -> object_selector, this component then publishes # pose in base_link reference frame when e_trigger is sent (next state) smach.StateMachine.add( 'SELECT_OBJECT', select_object( '/mcr_perception/object_selector/input/object_name'), transitions={'success': 'OPEN_GRIPPER'}) # open gripper smach.StateMachine.add( 'OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'GENERATE_OBJECT_POSE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add( 'GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'), transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'}) # waits for object selector response, if failure this means that the string published previously was not found in object list smach.StateMachine.add( 'CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0), transitions={ 'success': 'RECONFIGURE_PREGRASP_PARAMS', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # reconfigure pregrasp planner for pick smach.StateMachine.add( 'RECONFIGURE_PREGRASP_PARAMS', gbs.set_named_config('pick_normal_pregrasp'), transitions={ 'success': 'PLAN_ARM_MOTION', 'failure': 'SET_ACTION_LIB_FAILURE', 'timeout': 'RECONFIGURE_PREGRASP_PARAMS' }) # based on a published pose, calls pregrasp planner to generate a graspable pose and wait for the result of the pregrasp planner smach.StateMachine.add( 'PLAN_ARM_MOTION', gbs.send_and_wait_events_combined( event_in_list=[('/pregrasp_planner_pipeline/event_in', 'e_start')], event_out_list=[('/pregrasp_planner_pipeline/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', 'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE', 'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE' }) # pregrasp planner was successful, so lets stop it since its work is done smach.StateMachine.add('STOP_PLAN_ARM_MOTION', send_event( '/pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) # pregrasp planner failed or timeout, stop the component and then return overall failure smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION_WITH_FAILURE', send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) # move arm to pregrasp planned pose and wait for its response smach.StateMachine.add( 'MOVE_ARM_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/move_arm_planned/event_in', 'e_start')], event_out_list=[('/move_arm_planned/event_out', 'e_success', True)], timeout_duration=7), transitions={ 'success': 'STOP_MOVE_ARM_TO_OBJECT', 'timeout': 'STOP_MOVE_ARM_TO_OBJECT', 'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE' }) # send stop event_in to arm motion component and return failure smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', send_event('/move_arm_planned/event_in', 'e_stop'), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) # send stop event_in to arm motion component smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', send_event('/move_arm_planned/event_in', 'e_stop'), transitions={'success': 'CLOSE_GRIPPER'}) # close gripper smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'MOVE_ARM_TO_HOLD'}) # check if gripper changed in position, if not then close again # smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined( # event_in_list=[('/gripper_state_monitor/event_in','e_trigger')], # event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)], # timeout_duration=5), # transitions={'success':'MOVE_ARM_TO_HOLD', # 'timeout':'CLOSE_GRIPPER', # 'failure':'CLOSE_GRIPPER'}) # move arm to HOLD position smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'MOVE_ARM_TO_HOLD' }) # optocoupler check if robot has a object in the gripper, if not then pick failed # smach.StateMachine.add('VERIFY_OBJECT_GRASPED_OPTOCOUPLER', gbs.send_and_wait_events_combined( # event_in_list=[('/optocoupler_gripper_status/event_in','e_trigger')], # event_out_list=[('/optocoupler_gripper_status/event_out','e_grasped', True)], # timeout_duration=5), # transitions={'success':'SET_ACTION_LIB_SUCCESS', # 'timeout':'SET_ACTION_LIB_FAILURE', # 'failure':'SET_ACTION_LIB_FAILURE'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='pick_object_server', action_spec=PickObjectAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='pick_object_goal', feedback_key='pick_object_feedback', result_key='pick_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def tasker(): rospy.init_node('tasker') wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state runner = SMACHRunner(rgoap_subclasses) ## sub machines sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'], connector_outcome='succeeded') with sq_move_to_new_goal: Sequence.add('WAIT_FOR_GOAL', wfg) Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner)) sq_autonomous_rgoap = Sequence(outcomes=['preempted'], connector_outcome='succeeded') with sq_autonomous_rgoap: Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach()) Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner), transitions={'succeeded':'SLEEP_UNTIL_ENABLED', 'aborted':'SLEEP'}) Sequence.add('SLEEP', SleepState(5), transitions={'succeeded':'SLEEP_UNTIL_ENABLED'}) ## tasker machine sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted', 'field_error', 'undefined_task'], input_keys=['task_goal']) with sm_tasker: ## add all tasks to be available # states using rgoap StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal) StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner)) StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap) StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner)) # states from uashh_smach StateMachine.add('LOOK_AROUND', get_lookaround_smach()) StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True)) StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True)) StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach()) StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach()) StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach()) StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach()) StateMachine.add('SLEEP_FIVE_SEC', SleepState(5)) ## now the task receiver is created and automatically links to ## all task states added above task_states_labels = sm_tasker.get_children().keys() task_states_labels.sort() # sort alphabetically and then by _RGOAP task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True) task_receiver_transitions = {'undefined_outcome':'undefined_task'} task_receiver_transitions.update({l:l for l in task_states_labels}) StateMachine.add('TASK_RECEIVER', UserDataToOutcomeState(task_states_labels, 'task_goal', lambda ud: ud.task_id), task_receiver_transitions) sm_tasker.set_initial_state(['TASK_RECEIVER']) rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels)) pub = rospy.Publisher('/task/available_tasks', String, latch=True) thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1)) asw = ActionServerWrapper('activate_task', TaskActivationAction, wrapped_container=sm_tasker, succeeded_outcomes=['succeeded'], aborted_outcomes=['aborted', 'undefined_task'], preempted_outcomes=['preempted'], goal_key='task_goal' ) # Create and start the introspection server sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT') sis.start() asw.run_server() rospy.spin() sis.stop()
def main(): rospy.init_node("move_base_safe_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED", "OVERALL_PREEMPTED"], input_keys=["goal"], output_keys=["feedback", "result"], ) with sm: smach.StateMachine.add( "CHECK_IF_BARRIER_TAPE_ENABLED", CheckDontBeSafe(), transitions={ "safe": "START_BARRIER_TAPE_DETECTION", "unsafe": "SETUP_MOVE_BASE", }, ) smach.StateMachine.add( "START_BARRIER_TAPE_DETECTION", gbs.send_event([( "/mir_perception/barrier_tape_detection/event_in", "e_start", )]), transitions={"success": "MOVE_ARM_TO_DETECT_BARRIER_TAPE"}, ) smach.StateMachine.add( "MOVE_ARM_TO_DETECT_BARRIER_TAPE", gms.move_arm("barrier_tape", blocking=False), transitions={ "succeeded": "SETUP_MOVE_BASE", "failed": "MOVE_ARM_TO_DETECT_BARRIER_TAPE" }, ) # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add( "SETUP_MOVE_BASE", SetupMoveBase("/move_base_wrapper/pose_in"), transitions={ "succeeded": "SET_DIRECT_BASE_CONTROLLER_PARAMETERS", "failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "SET_DIRECT_BASE_CONTROLLER_PARAMETERS", gbs.set_named_config("dbc_move_base"), transitions={ "success": "START_MOVE_BASE", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "START_MOVE_BASE", StartMoveBase( event_in_topic="/move_base_wrapper/event_in", event_out_topic="/move_base_wrapper/event_out", timeout_duration=50, ), transitions={ "success": "PREPARE_ARM_FOR_NEXT_ACTION", "timeout": "RESET_BARRIER_TAPE", "failure": "RESET_BARRIER_TAPE", "stopped": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "RESET_BARRIER_TAPE", gbs.send_event([( "/mir_perception/barrier_tape_detection/event_in", "e_reset", ), ( "/move_base_wrapper/event_in", "e_stop", )]), transitions={"success": "SETUP_MOVE_BASE_AGAIN"}, ) smach.StateMachine.add( "SETUP_MOVE_BASE_AGAIN", SetupMoveBase("/move_base_wrapper/pose_in"), transitions={ "succeeded": "START_MOVE_BASE_AGAIN", "failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "START_MOVE_BASE_AGAIN", gbs.send_and_wait_events_combined( event_in_list=[("/move_base_wrapper/event_in", "e_start")], event_out_list=[("/move_base_wrapper/event_out", "e_success", True)], timeout_duration=50, ), transitions={ "success": "PREPARE_ARM_FOR_NEXT_ACTION", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) # send arm to a position depending on next action smach.StateMachine.add( "PREPARE_ARM_FOR_NEXT_ACTION", PrepareArmForNextAction(), transitions={ "succeeded": "MOVE_ARM_FOR_NEXT_ACTION", "skipped": "REACH_DESTINATION_PRECISELY", "failed": "PREPARE_ARM_FOR_NEXT_ACTION", }, ) smach.StateMachine.add( "MOVE_ARM_FOR_NEXT_ACTION", gms.move_arm(blocking=False), transitions={ "succeeded": "REACH_DESTINATION_PRECISELY", "failed": "REACH_DESTINATION_PRECISELY" }, ) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add( "REACH_DESTINATION_PRECISELY", gbs.send_and_wait_events_combined( event_in_list=[ ( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_start", ), ], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_success", True, )], timeout_duration=5, ), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={ "success": "STOP_CONTROLLER_WITH_SUCCESS", "timeout": "STOP_CONTROLLER_WITH_SUCCESS", "failure": "STOP_CONTROLLER_WITH_FAILURE", }, ) # stop controller with success smach.StateMachine.add( "STOP_CONTROLLER_WITH_SUCCESS", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_stop", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_stopped", True, )], timeout_duration=1, ), transitions={ "success": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) # stop controller with failure smach.StateMachine.add( "STOP_CONTROLLER_WITH_FAILURE", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_stop", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_stopped", True, )], timeout_duration=1, ), transitions={ "success": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", gbs.send_event([("/mir_perception/barrier_tape_detection/event_in", "e_stop"), ( "/move_base_wrapper/event_in", "e_stop", )]), transitions={"success": "OVERALL_FAILED"}, ) smach.StateMachine.add( "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", gbs.send_event([("/mir_perception/barrier_tape_detection/event_in", "e_stop")]), transitions={"success": "ALIGN_WITH_WORKSPACE"}, ) smach.StateMachine.add( "ALIGN_WITH_WORKSPACE", AlignWithWorkspace(), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "OVERALL_SUCCESS", }, ) state_publisher = rospy.Publisher('~current_state', String, queue_size=1) sm.register_transition_cb(transition_cb, [state_publisher]) sm.register_start_cb(start_cb, [state_publisher]) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = smach_ros.IntrospectionServer("move_base_safe_smach_viewer", sm, "/MOVE_BASE_SAFE_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="move_base_safe_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["OVERALL_PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node("perceive_cavity_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) sm.userdata.next_arm_pose_index = 0 # Open the container with sm: # approach to platform smach.StateMachine.add( "SETUP", Setup(), transitions={"succeeded": "PUBLISH_REFERENCE_FRAME_FOR_WBC"}, ) # generates a pose based on the previous string object topic received smach.StateMachine.add( "PUBLISH_REFERENCE_FRAME_FOR_WBC", gbs.send_event([("/static_transform_publisher_node/event_in", "e_start")]), transitions={"success": "START_POSE_SELECTOR"}, ) # start the cavity pose selector to accumulate the poses smach.StateMachine.add( "START_POSE_SELECTOR", gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in", "e_start")]), transitions={"success": "LOOK_AROUND"}, ) # move arm to selected pose smach.StateMachine.add( "LOOK_AROUND", gms.move_arm("look_at_workspace_from_near", blocking=True), transitions={ "succeeded": "RECOGNIZE_CAVITIES", "failed": "LOOK_AROUND", }, ) # trigger perception pipeline smach.StateMachine.add( "RECOGNIZE_CAVITIES", gps.find_cavities(retries=1), transitions={ "cavities_found": "POPULATE_RESULT_WITH_CAVITIES", "no_cavities_found": "OVERALL_FAILED", }, ) # populate action server result with perceived objects smach.StateMachine.add( "POPULATE_RESULT_WITH_CAVITIES", PopulateResultWithCavities(), transitions={"succeeded": "OVERALL_SUCCESS"}, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = IntrospectionServer("perceive_cavity_smach_viewer", sm, "/PERCEIVE_CAVITY_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="perceive_cavity_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(mokeup=False): # Open the container rospy.init_node('pick_object_wbc_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['pick_object_wbc_goal'], output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result']) with sm: if not mokeup: # publish object as string to mcr_perception_selectors -> object_selector, this component then publishes # pose in base_link reference frame when e_trigger is sent (next state) smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'), transitions={'success':'OPEN_GRIPPER'}) # open gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'GENERATE_OBJECT_POSE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'), transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'}) # waits for object selector response, if failure this means that the string published previously was not found in object list smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0), transitions={'success':'COLLISION_VELOCITY_CHECKER', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('COLLISION_VELOCITY_CHECKER', gbs.send_and_wait_events_combined( event_in_list=[("/base_collision_checker_pipeline/pose_transformer/event_in",'e_start'), ('/base_collision_checker_pipeline/base_collision_checker_node/event_in','e_start')], event_out_list=[('/base_collision_checker_pipeline/pose_transformer/event_out','e_success', True), ('/base_collision_checker_pipeline/base_collision_checker_node/event_out','e_success', True)], timeout_duration=30), transitions={'success':'STOP_COLLISION_CHECKER', 'timeout':'STOP_COLLISION_CHECKER_FAILURE', 'failure':'STOP_COLLISION_CHECKER_FAILURE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('STOP_COLLISION_CHECKER', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'), transitions={'success':'STOP_POSE_TRANSFORMER'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('STOP_POSE_TRANSFORMER', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'), transitions={'success':'SET_ACTION_LIB_SUCCESS'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('STOP_COLLISION_CHECKER_FAILURE', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'), transitions={'success':'STOP_POSE_TRANSFORMER_FAILURE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('STOP_POSE_TRANSFORMER_FAILURE', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'), transitions={'success':'SET_ACTION_LIB_FAILURE'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded':'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'wbc_pick_object_server', action_spec = PickObjectWBCAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'pick_object_wbc_goal', feedback_key = 'pick_object_wbc_feedback', result_key = 'pick_object_wbc_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('perceive_location_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['perceive_location_goal'], output_keys=['perceive_location_feedback', 'perceive_location_result']) # Open the container with sm: # approach to platform smach.StateMachine.add( 'GET_GOAL', getGoal(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'}) #transitions={'succeeded':'START_OBJECT_LIST_MERGER'}) # generates a pose based on the previous string object topic received smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'START_OBJECT_LIST_MERGER'}) smach.StateMachine.add( 'START_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_start')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'LOOK_AT_WORKSPACE_LEFT', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'), smach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_left'), transitions={ 'succeeded': 'SUBSCRIBE_TO_POINT_CLOUD', 'failed': 'LOOK_AT_WORKSPACE_LEFT' }) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'RECOGNIZE_OBJECTS_LEFT'}) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_LEFT', gps.find_objects(retries=1), transitions={ 'objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT', 'no_objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT' }, #transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_2', # 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_2'}, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD_2', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'LOOK_AT_WORKSPACE_STRAIGHT'}) # send arm to a position in which the depth camera can see the platform smach.StateMachine.add('LOOK_AT_WORKSPACE_STRAIGHT', gms.move_arm('look_at_workspace'), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS_STRAIGHT', 'failed': 'LOOK_AT_WORKSPACE_STRAIGHT' }) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_STRAIGHT', gps.find_objects(retries=1), transitions={ 'objects_found': 'LOOK_AT_WORKSPACE_RIGHT', 'no_objects_found': 'LOOK_AT_WORKSPACE_RIGHT' }, #transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_3', # 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_3'}, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD_3', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'LOOK_AT_WORKSPACE_RIGHT'}) # send arm to a position in which the depth camera can see the platform smach.StateMachine.add('LOOK_AT_WORKSPACE_RIGHT', gms.move_arm('look_at_workspace_right'), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS_RIGHT', 'failed': 'LOOK_AT_WORKSPACE_RIGHT' }) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_RIGHT', gps.find_objects(retries=1), transitions={ 'objects_found': 'STOP_OBJECT_LIST_MERGER', 'no_objects_found': 'STOP_OBJECT_LIST_MERGER' }, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'STOP_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_stop')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD' }) smach.StateMachine.add( 'UNSUBSCRIBE_FROM_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/emtpty_topic')]), transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'}) smach.StateMachine.add( 'PUBLISH_MERGED_OBJECT_LIST', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_trigger')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_done', True) ], timeout_duration=5), transitions={ 'success': 'SET_ACTION_LIB_SUCCESS', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm, '/PERCEIVE_LOCATION_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_location_server', action_spec=PerceiveLocationAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='perceive_location_goal', feedback_key='perceive_location_feedback', result_key='perceive_location_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('perceive_location_server') sleep_time = rospy.get_param('~sleep_time', 1.0) # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['goal'], output_keys=['feedback', 'result']) # Open the container sm.userdata.arm_pose_list = [ 'look_at_workspace_from_near', 'look_at_workspace_from_near_left', 'look_at_workspace_from_near_right' ] sm.userdata.arm_pose_index = 0 base_x_offset = rospy.get_param('~base_x_offset', 0.0) base_y_offset = rospy.get_param('~base_y_offset', 0.25) base_theta_offset = rospy.get_param('~base_theta_offset', 0.0) sm.userdata.base_pose_list = [{ 'x': 0.0, 'y': 0.0, 'theta': 0.0 }, { 'x': base_x_offset, 'y': base_y_offset, 'theta': base_theta_offset }, { 'x': base_x_offset, 'y': -base_y_offset, 'theta': -base_theta_offset }] sm.userdata.base_pose_index = 0 with sm: # approach to platform smach.StateMachine.add( 'SETUP', Setup(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'}) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'SET_DBC_PARAMS'}) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'), transitions={ 'success': 'START_OBJECT_LIST_MERGER', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'START_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/object_list_merger/event_in', 'e_start'), ('/mcr_perception/object_selector/event_in', 'e_start') ], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'GET_MOTION_TYPE', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add('GET_MOTION_TYPE', GetMotionType(), transitions={ 'base_motion': 'SET_APPROPRIATE_BASE_POSE', 'arm_motion': 'SET_APPROPRIATE_ARM_POSE' }) smach.StateMachine.add('SET_APPROPRIATE_BASE_POSE', SetupMoveBaseWithDBC(), transitions={ 'pose_set': 'MOVE_BASE_WITH_DBC', 'tried_all': 'POPULATE_RESULT_WITH_OBJECTS' }) smach.StateMachine.add( 'MOVE_BASE_WITH_DBC', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'MOVE_ARM', 'timeout': 'STOP_DBC', 'failure': 'STOP_DBC' }) smach.StateMachine.add('SET_APPROPRIATE_ARM_POSE', SetupMoveArm(), transitions={ 'pose_set': 'MOVE_ARM', 'tried_all': 'POPULATE_RESULT_WITH_OBJECTS' }) # move arm to appropriate position smach.StateMachine.add('MOVE_ARM', gms.move_arm_and_gripper('open'), transitions={ 'succeeded': 'START_OBJECT_RECOGNITION', 'failed': 'MOVE_ARM' }) # New perception pipeline state machine smach.StateMachine.add( 'START_OBJECT_RECOGNITION', gbs.send_and_wait_events_combined( event_in_list=[ ('/mir_perception/multimodal_object_recognition/event_in', 'e_start') ], event_out_list=[ ('/mir_perception/multimodal_object_recognition/event_out', 'e_done', True) ], timeout_duration=10), transitions={ 'success': 'STOP_RECOGNITION', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'STOP_RECOGNITION', gbs.send_and_wait_events_combined( event_in_list=[ ('/mir_perception/multimodal_object_recognition/event_in', 'e_stop') ], event_out_list=[ ('/mir_perception/multimodal_object_recognition/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'STOP_OBJECT_LIST_MERGER', 'timeout': 'STOP_OBJECT_LIST_MERGER', 'failure': 'STOP_OBJECT_LIST_MERGER' }) smach.StateMachine.add( 'STOP_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_stop')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'PUBLISH_MERGED_OBJECT_LIST', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'PUBLISH_MERGED_OBJECT_LIST', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_trigger')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_done', True) ], timeout_duration=5), transitions={ 'success': 'CHECK_IF_OBJECTS_FOUND', 'timeout': 'CHECK_IF_OBJECTS_FOUND', 'failure': 'CHECK_IF_OBJECTS_FOUND' }) smach.StateMachine.add( 'CHECK_IF_OBJECTS_FOUND', gbs.send_and_wait_events_combined( event_in_list=[( '/mcr_perception/object_list_merger/object_found_event_in', 'e_trigger')], event_out_list=[( '/mcr_perception/object_list_merger/object_found_event_out', 'e_objects_found', True)], timeout_duration=10.0), transitions={ 'success': 'POPULATE_RESULT_WITH_OBJECTS', 'timeout': 'OVERALL_FAILED', 'failure': 'GET_MOTION_TYPE' }) # populate action server result with perceived objects smach.StateMachine.add('POPULATE_RESULT_WITH_OBJECTS', PopulateResultWithObjects(), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add( 'STOP_DBC', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=10), transitions={ 'success': 'OVERALL_FAILED', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = IntrospectionServer('perceive_location_smach_viewer', sm, '/PERCEIVE_LOCATION_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_location_server', action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='goal', feedback_key='feedback', result_key='result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('find_object') sm = StateMachine(outcomes=['success', 'aborted', 'preempted'], input_keys=['goal'], output_keys=['result']) with sm: sm.userdata.feedback = FindObjectFeedback() sm.userdata.result = FindObjectResult() sm.userdata.min_confidence = 0.5 sm.userdata.recognised_objects = object_recognition_msgs.msg.RecognizedObjectArray( ) sm.userdata.object_names = list() # sm.userdata.objects_info = list() sm.userdata.error_code = int() sm.userdata.error_message = str() sm.userdata.tf_listener = tf.TransformListener() smach.StateMachine.add('Prepare', Prepare(), remapping={ 'goal': 'goal', 'min_confidence': 'min_confidence', 'table_pose': 'table_pose', 'look_around': 'look_around', 'feedback': 'feedback', 'tf_listener': 'tf_listener' }, transitions={'prepared': 'FindObject'}) sm_find_object = find_object_sm.createSM() smach.StateMachine.add('FindObject', sm_find_object, remapping={ 'table_pose': 'table_pose', 'look_around': 'look_around', 'min_confidence': 'min_confidence', 'recognised_objects': 'recognised_objects', 'object_names': 'object_names', 'error_message': 'error_message', 'error_code': 'error_code', 'tf_listener': 'tf_listener' }, transitions={ 'object_found': 'Finalise', 'no_objects_found': 'Finalise', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('Finalise', Finalise(), remapping={ 'recognised_objects': 'recognised_objects', 'object_names': 'object_names', 'error_message': 'error_message', 'error_code': 'error_code', 'result': 'result', 'feedback': 'feedback', 'result': 'result' }, transitions={'prepared': 'success'}) asw = ActionServerWrapper('find_object', FindObjectAction, wrapped_container=sm, goal_key='goal', feedback_key='feedback', result_key='result', succeeded_outcomes=['success'], aborted_outcomes=['aborted'], preempted_outcomes=['preempted']) asw.run_server() rospy.spin()
def main(): rospy.init_node('unstage_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['unstage_object_goal'], output_keys=['unstage_object_feedback', 'unstage_object_result']) with sm: #add states to the container smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE', SetupMoveArm('pre'), transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'}) smach.StateMachine.add('MOVE_ARM_PRE_STAGE', gms.move_arm(), transitions={ 'succeeded': 'OPEN_GRIPPER', 'failed': 'MOVE_ARM_PRE_STAGE' }) smach.StateMachine.add( 'OPEN_GRIPPER', gms.control_gripper('open_release'), transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE'}) smach.StateMachine.add('SETUP_MOVE_ARM_STAGE', SetupMoveArm('final'), transitions={'succeeded': 'MOVE_ARM_STAGE'}) smach.StateMachine.add('MOVE_ARM_STAGE', gms.move_arm(), transitions={ 'succeeded': 'CLOSE_GRIPPER', 'failed': 'MOVE_ARM_STAGE' }) smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'MOVE_ARM_TO_HOLD'}) # smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined( # event_in_list=[('/gripper_state_monitor/event_in','e_trigger')], # event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)], # timeout_duration=5), # transitions={'success':'MOVE_ARM_TO_HOLD', # 'timeout':'CLOSE_GRIPPER', # 'failure':'CLOSE_GRIPPER'}) smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'MOVE_ARM_TO_HOLD' }) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) # smach viewer sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm, '/UNSTAGE_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='unstage_object_server', action_spec=UnStageObjectAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='unstage_object_goal', feedback_key='unstage_object_feedback', result_key='unstage_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def __init__(self, heritage): # Creation of State Machine and definition of its outcomes self.agent_sm = StateMachine(outcomes=['completed', 'failed']) with self.agent_sm: # Initialization of the dictionary containing every Action Service Wrapper self.asw_dicc = {} ### ACTION SERVER ADVERTISING ### # Add a state where drone does nothing but wait to receive a service order StateMachine.add( 'action_server_advertiser', CBState(self.action_server_advertiser_stcb, cb_kwargs={ 'heritage': heritage, 'asw_dicc': self.asw_dicc }), {'completed': 'completed'}) # Every action service wrapper is similar following a structure defined by SMACH. # Each wrapper is defined and added to # To understand what they do, see callbacks below ### TAKE-OFF STATE MACHINE & WRAPPER ### self.take_off_sm = StateMachine(outcomes=['completed', 'failed'], input_keys=['action_goal']) self.asw_dicc['take_off'] = ActionServerWrapper( '/magna/GS_Agent_{}/take_off_command'.format(heritage.ID), TakeOffAction, self.take_off_sm, ['completed'], ['failed'], ['preempted'], goal_key='action_goal', result_key='action_result') with self.take_off_sm: StateMachine.add( 'take_off', CBState(self.take_off_stcb, input_keys=['action_goal'], cb_kwargs={'heritage': heritage}), {'completed': 'completed'}) ### LAND STATE MACHINE & WRAPPER ### self.land_sm = StateMachine(outcomes=['completed', 'failed'], input_keys=['action_goal']) self.asw_dicc['land'] = ActionServerWrapper( '/magna/GS_Agent_{}/land_command'.format(heritage.ID), LandAction, self.land_sm, ['completed'], ['failed'], ['preempted'], goal_key='action_goal', result_key='action_result') with self.land_sm: StateMachine.add( 'land', CBState(self.land_stcb, input_keys=['action_goal'], cb_kwargs={'heritage': heritage}), {'completed': 'completed'}) ### BASIC MOVE STATE MACHINE & WRAPPER ### self.basic_move_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['basic_move'] = ActionServerWrapper( '/magna/GS_Agent_{}/basic_move_command'.format(heritage.ID), BasicMoveAction, self.basic_move_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.basic_move_sm: StateMachine.add( 'basic_move', CBState(self.basic_move_stcb, input_keys=['action_goal', 'action_result'], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### SET MISSION STATE MACHINE & WRAPPER ### self.set_mission_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['set_mission'] = ActionServerWrapper( '/magna/GS_Agent_{}/set_mission_command'.format(heritage.ID), SetMissionAction, self.set_mission_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.set_mission_sm: StateMachine.add( 'set_mission', CBState(self.set_mission_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### FOLLOW PATH STATE MACHINE & WRAPPER ### self.follow_path_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_path'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_path_command'.format(heritage.ID), FollowPathAction, self.follow_path_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_path_sm: StateMachine.add( 'follow_path', CBState(self.follow_path_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) # StateMachine.add('to_wp', self.follow_path_sm, # {'completed':'action_server_advertiser'}) ### FOLLOW Agent AD STATE MACHINE & WRAPPER### self.follow_agent_ad_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_agent_ad'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_agent_ad_command'.format( heritage.ID), FollowAgentADAction, self.follow_agent_ad_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_agent_ad_sm: StateMachine.add( 'follow_agent_ad', CBState(self.follow_agent_ad_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### FOLLOW Agent AP STATE MACHINE & WRAPPER### self.follow_agent_ap_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_agent_ap'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_agent_ap_command'.format( heritage.ID), FollowAgentAPAction, self.follow_agent_ap_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_agent_ap_sm: StateMachine.add( 'follow_agent_ap', CBState(self.follow_agent_ap_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) if heritage.smach_view == True: sis = smach_ros.IntrospectionServer( 'magna/Agent_{}_introspection'.format(heritage.ID), self.agent_sm, '/Agent_{}'.format(heritage.ID)) sis.start()
def main(): rospy.init_node("stage_object_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) with sm: # add states to the container smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE", gms.move_arm("stage_intermediate"), transitions={ "succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE_2", gms.move_arm("stage_intermediate_2"), transitions={ "succeeded": "SETUP_MOVE_ARM_PRE_STAGE", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2", }, ) smach.StateMachine.add( "SETUP_MOVE_ARM_PRE_STAGE", SetupMoveArm("pre"), transitions={ "succeeded": "MOVE_ARM_PRE_STAGE", "failed": "SETUP_MOVE_ARM_PRE_STAGE", }, ) smach.StateMachine.add( "MOVE_ARM_PRE_STAGE", gms.move_arm(), transitions={ "succeeded": "SETUP_MOVE_ARM_STAGE", "failed": "MOVE_ARM_PRE_STAGE", }, ) smach.StateMachine.add( "SETUP_MOVE_ARM_STAGE", SetupMoveArm("final"), transitions={ "succeeded": "MOVE_ARM_STAGE", "failed": "SETUP_MOVE_ARM_STAGE", }, ) smach.StateMachine.add( "MOVE_ARM_STAGE", gms.move_arm(), transitions={ "succeeded": "OPEN_GRIPPER", "failed": "MOVE_ARM_STAGE" }, ) smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open_narrow"), transitions={"succeeded": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN"}, ) smach.StateMachine.add( "SETUP_MOVE_ARM_PRE_STAGE_AGAIN", SetupMoveArm("pre"), transitions={ "succeeded": "MOVE_ARM_PRE_STAGE_AGAIN", "failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN", }, ) smach.StateMachine.add( "MOVE_ARM_PRE_STAGE_AGAIN", gms.move_arm(blocking=False), transitions={ "succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL", "failed": "MOVE_ARM_PRE_STAGE_AGAIN", }, ) smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL", gms.move_arm("stage_intermediate_2"), transitions={ "succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL", }, ) smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL", gms.move_arm("stage_intermediate"), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL", }, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = smach_ros.IntrospectionServer("stage_object_smach_viewer", sm, "/STAGE_OBJECT_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="stage_object_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(mokeup=False): # Open the container rospy.init_node('pick_object_wbc_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['pick_object_wbc_goal'], output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result']) with sm: if not mokeup: smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'), transitions={'success':'OPEN_GRIPPER'}) # open gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'GENERATE_OBJECT_POSE'}) # generates a pose based on the previous string object topic received smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'), transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'}) # waits for object selector response, if failure this means that the string published previously was not found in object list smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0), transitions={'success':'SET_DBC_PARAMS', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'), transitions={'success':'MOVE_ROBOT_TO_OBJECT', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/wbc/event_in','e_start')], event_out_list=[('/wbc/event_out','e_success', True)], timeout_duration=25), transitions={'success':'STOP_MOVE_ROBOT_TO_OBJECT', 'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'}) # execute robot motion smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]), transitions={'success':'CLOSE_GRIPPER'}) smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]), transitions={'success':'SET_ACTION_LIB_FAILURE'}) # close gripper smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'MOVE_ARM_TO_HOLD'}) # move arm to HOLD position smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"), transitions={'succeeded':'VERIFY_OBJECT_GRASPED', 'failed':'MOVE_ARM_TO_HOLD'}) smach.StateMachine.add('VERIFY_OBJECT_GRASPED', gbs.send_and_wait_events_combined( event_in_list=[('/gripper_controller/grasp_monitor/event_in','e_trigger')], event_out_list=[('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5), transitions={'success':'SET_ACTION_LIB_SUCCESS', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded':'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'wbc_pick_object_server', action_spec = PickObjectWBCAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'pick_object_wbc_goal', feedback_key = 'pick_object_wbc_feedback', result_key = 'pick_object_wbc_result') # Run the server in a background thread asw.run_server() rospy.spin()