def execute(self, userdata): # give feedback feedback = PickObjectFeedback() feedback.current_state = 'CBT_GRASP' userdata.pick_object_feedback = feedback gms_o.control_gripper('close') #gms_o.gripper_command.go() rospy.sleep(3.0) return 'succeeded'
def execute(self, userdata): #TODO TIMING # give feedback feedback = PickObjectFeedback() feedback.current_state = 'CBT_RELEASE' userdata.pick_object_feedback = feedback gms_o.control_gripper("open") #rospy.sleep(3.5) #gms_o.gripper_command.set_named_target("open") #gms_o.gripper_command.go() rospy.sleep(2.0) return 'succeeded'
def __init__(self): smach.StateMachine.__init__( self, outcomes=['success', 'failure'], output_keys=['is_object_grasped', 'end_effector_pose']) with self: smach.StateMachine.add( 'OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'LOOK_AT_WORKSPACE'}) smach.StateMachine.add('LOOK_AT_WORKSPACE', gms.move_arm('look_at_workspace'), transitions={ 'succeeded': 'success', 'failed': 'failure' })
def __init__(self): smach.StateMachine.__init__( self, outcomes=["success", "failure"], output_keys=["is_object_grasped", "end_effector_pose"], ) with self: smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "LOOK_AT_WORKSPACE"}, ) smach.StateMachine.add( "LOOK_AT_WORKSPACE", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "success", "failed": "failure" }, )
def main(): # Open the container rospy.init_node("pick_object_wbc_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": "OPEN_GRIPPER", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "SET_DBC_PARAMS"}, ) smach.StateMachine.add( "SET_DBC_PARAMS", gbs.set_named_config("dbc_pick_object"), transitions={ "success": "MOVE_ROBOT_AND_PICK", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) # whole body control command. It moves direct base controller and # calls pre-grasp planner, and (optionally) moves arm to object pose smach.StateMachine.add( "MOVE_ROBOT_AND_PICK", 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=50, ), transitions={ "success": "CLOSE_GRIPPER", "timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", "failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", gbs.send_event([ ("/waypoint_trajectory_generation/event_in", "e_start"), ("/wbc/event_in", "e_stop"), ]), transitions={"success": "OVERALL_FAILED"}, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE"}, ) # move arm to stage_intemediate position smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE", gms.move_arm("stage_intermediate"), transitions={ "succeeded": "VERIFY_OBJECT_GRASPED", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE", }, ) 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": "OVERALL_SUCCESS", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = 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=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 __init__(self, use_mockup=None): smach.StateMachine.__init__( self, outcomes=[ "pose_skipped_but_platform_limit_reached", "no_more_free_poses", "no_more_free_poses_at_robot_platf", "no_more_task_for_given_type", ], input_keys=[ "base_pose_to_approach", "desired_distance_to_workspace", "found_objects", "lasttask", "move_arm_to", "move_base_by", "next_arm_pose_index", "object_pose", "object_to_be_adjust_to", "object_to_grasp", "objects_to_be_grasped", "prev_vs_result", "rear_platform_free_poses", "rear_platform_occupied_poses", "recognized_objects", "source_visits", "task_list", "test", "vscount", ], output_keys=[ "base_pose_to_approach", "found_objects", "lasttask", "move_arm_to", "move_base_by", "next_arm_pose_index", "object_to_be_adjust_to", "object_to_grasp", "objects_to_be_grasped", "prev_vs_result", "rear_platform_free_poses", "rear_platform_occupied_poses", "source_visits", "task_list", "test", "vscount", ], ) self.use_mockup = use_mockup self.use_visual_servoing = True with self: smach.StateMachine.add( "SELECT_SOURCE_SUBTASK", btts.select_btt_subtask(type="source"), transitions={ "task_selected": "MOVE_TO_SOURCE_LOCATION_SAFE", "no_more_task_for_given_type": "no_more_task_for_given_type", }, ) # required before any call to gns.approach_pose, moves arm within footprint smach.StateMachine.add( "MOVE_TO_SOURCE_LOCATION_SAFE", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "MOVE_TO_SOURCE_LOCATION", "failed": "MOVE_TO_SOURCE_LOCATION_SAFE", }, ) smach.StateMachine.add( "MOVE_TO_SOURCE_LOCATION", gns.approach_pose(), transitions={ "succeeded": "STOP_ALL_COMPONENTS_AT_START", "failed": "MOVE_TO_SOURCE_LOCATION", }, ) smach.StateMachine.add( "STOP_ALL_COMPONENTS_AT_START", gbs.send_event([ ( "/gripper_to_object_pose_error_calculator/event_in", "e_stop", ), ( "/mcr_common/relative_displacement_calculator/event_in", "e_stop", ), ("/pregrasp_planner/event_in", "e_stop"), ( "/mcr_navigation/relative_base_controller/event_in", "e_stop", ), ("/planned_motion/event_in", "e_stop"), ]), transitions={"success": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE"}, ) smach.StateMachine.add( "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE", gns.adjust_to_workspace(0.2), transitions={ "succeeded": "SELECT_NEXT_LOOK_POSE", "failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE", }, ) smach.StateMachine.add( "SELECT_NEXT_LOOK_POSE", gms.select_arm_pose([ "look_at_workspace_right", "look_at_workspace", "look_at_workspace_left", ]), transitions={ "succeeded": "LOOK_AROUND", "failed": "RECOGNIZE_OBJECTS", }, ) 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": "ACCUMULATE_RECOGNIZED_LISTS", "no_objects_found": "ACCUMULATE_RECOGNIZED_LISTS", }, remapping={"found_objects": "recognized_objects"}, ) smach.StateMachine.add( "ACCUMULATE_RECOGNIZED_LISTS", gps.accumulate_recognized_objects_list(), transitions={ "complete": "SELECT_OBJECT_TO_BE_GRASPED", "merged": "SELECT_NEXT_LOOK_POSE", }, ) smach.StateMachine.add( "SELECT_OBJECT_TO_BE_GRASPED", btts.select_object_to_be_grasped(), transitions={ "obj_selected": "COMPUTE_ARM_BASE_SHIFT_TO_OBJECT", "no_obj_selected": "SKIP_SOURCE_POSE", "no_more_free_poses_at_robot_platf": "no_more_free_poses_at_robot_platf", }, ) smach.StateMachine.add( "COMPUTE_ARM_BASE_SHIFT_TO_OBJECT", gbs.send_event([ ("/pregrasp_planner/event_in", "e_start"), ( "/gripper_to_object_pose_error_calculator/event_in", "e_start", ), ( "/mcr_common/relative_displacement_calculator/event_in", "e_start", ), ]), transitions={ "success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT" }, ) smach.StateMachine.add( "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT", gbs.wait_for_events([ ("/pregrasp_planner/event_out", "e_success", True), ( "/gripper_to_object_pose_error_calculator/event_out", "e_success", True, ), ( "/mcr_common/relative_displacement_calculator/event_out", "e_done", True, ), ]), transitions={ "success": "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT", "timeout": "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE", "failure": "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT", gbs.send_event([ ( "/gripper_to_object_pose_error_calculator/event_in", "e_stop", ), ("/pregrasp_planner/event_in", "e_stop"), ( "/mcr_common/relative_displacement_calculator/event_in", "e_stop", ), ]), transitions={"success": "MOVE_ARM_AND_BASE_TO_OBJECT"}, ) smach.StateMachine.add( "STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE", gbs.send_event([ ( "/gripper_to_object_pose_error_calculator/event_in", "e_stop", ), ("/pregrasp_planner/event_in", "e_stop"), ( "/mcr_common/relative_displacement_calculator/event_in", "e_stop", ), ]), transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"}, ) smach.StateMachine.add( "MOVE_ARM_AND_BASE_TO_OBJECT", gbs.send_event([ ( "/mcr_navigation/relative_base_controller/event_in", "e_start", ), ("/planned_motion/event_in", "e_start"), ]), transitions={"success": "WAIT_ARM_AND_BASE_TO_OBJECT"}, ) smach.StateMachine.add( "WAIT_ARM_AND_BASE_TO_OBJECT", gbs.wait_for_events([ ( "/mcr_navigation/relative_base_controller/event_out", "e_done", True, ), ( "/mcr_navigation/collision_velocity_filter/event_out", "e_zero_velocities_forwarded", False, ), ("/planned_motion/event_out", "e_success", True), ]), transitions={ "success": "STOP_MOVE_ARM_BASE_TO_OBJECT", "timeout": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE", "failure": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_MOVE_ARM_BASE_TO_OBJECT", gbs.send_event([ ( "/mcr_navigation/relative_base_controller/event_in", "e_stop", ), ("/planned_motion/event_in", "e_stop"), ]), # transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing transitions={"success": "GRASP_OBJ"}, ) # comment out if using visual servoing smach.StateMachine.add( "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE", gbs.send_event([ ( "/mcr_navigation/relative_base_controller/event_in", "e_stop", ), ("/planned_motion/event_in", "e_stop"), ]), transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"}, ) smach.StateMachine.add( "ALIGN_WITH_OBJECT_LOOP", mir_gbs.loop_for_vs(1), transitions={ "loop": "START_VISUAL_SERVOING", "continue": "GRASP_OBJ", }, ) smach.StateMachine.add( "START_VISUAL_SERVOING", gbs.send_event([("/mir_controllers/visual_servoing/event_in", "e_start")]), transitions={"success": "WAIT_VISUAL_SERVOING"}, ) smach.StateMachine.add( "WAIT_VISUAL_SERVOING", gbs.wait_for_events( [( "/mcr_monitoring/component_wise_pose_error_monitor/event_out", "e_stop", True, )], timeout_duration=30, ), transitions={ "success": "STOP_VISUAL_SERVOING", "timeout": "STOP_VISUAL_SERVOING_WITH_FAILURE", "failure": "STOP_VISUAL_SERVOING_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_VISUAL_SERVOING", gbs.send_event([("/mir_controllers/visual_servoing/event_in", "e_stop")]), transitions={"success": "SET_VISUAL_SERVOING_SUCCESS"}, ) smach.StateMachine.add( "STOP_VISUAL_SERVOING_WITH_FAILURE", gbs.send_event([("/mir_controllers/visual_servoing/event_in", "e_stop")]), transitions={"success": "SET_VISUAL_SERVOING_FAILURE"}, ) smach.StateMachine.add( "SET_VISUAL_SERVOING_SUCCESS", mir_gbs.set_vs_status(status=True), transitions={"success": "OPEN_GRIPPER_FOR_APPROACH_OBJECT"}, ) smach.StateMachine.add( "SET_VISUAL_SERVOING_FAILURE", mir_gbs.set_vs_status(status=False), transitions={"success": "SELECT_OBJECT_TO_BE_GRASPED"}, ) smach.StateMachine.add( "OPEN_GRIPPER_FOR_APPROACH_OBJECT", gms.control_gripper("open"), transitions={"succeeded": "APPROACH_OBJECT"}, ) smach.StateMachine.add( "APPROACH_OBJECT", gbs.send_event([( "/guarded_approach_pose/coordinator/event_in", "e_start", )]), transitions={"success": "WAIT_APPROACH_OBJECT"}, ) smach.StateMachine.add( "WAIT_APPROACH_OBJECT", gbs.wait_for_events( [( "/guarded_approach_pose/coordinator/event_out", "e_success", True, )], timeout_duration=6, ), transitions={ "success": "STOP_APPROACH_OBJECT", "timeout": "STOP_APPROACH_OBJECT", "failure": "STOP_APPROACH_OBJECT", }, ) smach.StateMachine.add( "STOP_APPROACH_OBJECT", gbs.send_event([("/guarded_approach_pose/coordinator/event_in", "e_stop")]), transitions={"success": "CLOSE_GRIPPER"}, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM"}, ) smach.StateMachine.add( "GRASP_OBJ", gms.linear_motion(operation="grasp"), transitions={ "succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM", "failed": "DELETE_FROM_RECOGNIZED_OBJECTS", }, ) # TODO: this could loop smach.StateMachine.add( "GRASP_OBJ_VISUAL_SERVOING", gms.linear_motion(operation="grasp", offset_x=-0.05), transitions={ "succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM", "failed": "DELETE_FROM_RECOGNIZED_OBJECTS", }, ) # TODO: this could loop smach.StateMachine.add( "PRE_PLACE_OBJ_ON_REAR_PLATFORM", btts.pre_place_obj_on_rear_platform_btt(), transitions={ "succeeded": "VERIFY_GRASPED", "no_more_free_poses": "no_more_free_poses", }, ) smach.StateMachine.add( "VERIFY_GRASPED", gbs.send_event([( "/gripper_controller/grasp_monitor/event_in", "e_trigger", )]), transitions={"success": "WAIT_FOR_VERIFY_GRASPED"}, ) smach.StateMachine.add( "WAIT_FOR_VERIFY_GRASPED", gbs.wait_for_events( [( "/gripper_controller/grasp_monitor/event_out", "e_object_grasped", True, )], timeout_duration=5, ), transitions={ "success": "PLACE_OBJ_ON_REAR_PLATFORM", "timeout": "OPEN_GRIPPER_FOR_GRASP_FAILURE", "failure": "OPEN_GRIPPER_FOR_GRASP_FAILURE", }, ) smach.StateMachine.add( "OPEN_GRIPPER_FOR_GRASP_FAILURE", gms.control_gripper("open"), transitions={"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS"}, ) # THESE ARE NEVER ENTERED? # It would mean we've grasped the object - and don't have somewhere to place it. # If we don't have somewhere to place it, we won't grasp in the first place? smach.StateMachine.add( "PLACE_OBJ_ON_REAR_PLATFORM", btts.place_obj_on_rear_platform_btt(), transitions={ "succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS", "no_more_free_poses": "no_more_free_poses", }, ) smach.StateMachine.add( "DELETE_FROM_RECOGNIZED_OBJECTS", btts.delete_from_recognized_objects(), transitions={"succeeded": "SELECT_OBJECT_TO_BE_GRASPED"}, remapping={"object_to_delete": "object_to_grasp"}, ) smach.StateMachine.add( "ADJUST_POSE_WRT_WORKSPACE_NEXT", gns.adjust_to_workspace(0.1), transitions={ "succeeded": "SELECT_OBJECT_TO_BE_GRASPED", "failed": "ADJUST_POSE_WRT_WORKSPACE_NEXT", }, ) smach.StateMachine.add( "NO_MORE_FREE_POSES_SAFE", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "no_more_free_poses", "failed": "SKIP_SOURCE_POSE_SAFE", }, ) # MISC STATES smach.StateMachine.add( "SKIP_SOURCE_POSE_SAFE", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "SKIP_SOURCE_POSE", "failed": "SKIP_SOURCE_POSE_SAFE", }, ) smach.StateMachine.add( "SKIP_SOURCE_POSE", btts.skip_pose("source"), transitions={ "pose_skipped": "SELECT_SOURCE_SUBTASK", "pose_skipped_but_platform_limit_reached": "pose_skipped_but_platform_limit_reached", }, )
def __init__(self, use_mockup=None): smach.StateMachine.__init__( self, outcomes=['success', 'failure'], input_keys=['is_object_grasped', 'end_effector_pose'], output_keys=['is_object_grasped', 'end_effector_pose']) with self: smach.StateMachine.add( 'SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('pregrasp_laying_object'), transitions={ 'success': 'PLAN_ARM_MOTION', 'failure': 'failure', 'timeout': 'SET_NAMED_CONFIG_PREGRASP' }) 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)]), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in', 'e_stop')]), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) 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=15), transitions={ 'success': 'STOP_MOVE_ARM_TO_OBJECT', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in', 'e_stop')]), transitions={'success': 'SAVE_GRIPPER_POSE'}) smach.StateMachine.add('SAVE_GRIPPER_POSE', mfs.save_gripper_pose(), transitions={'success': 'CLOSE_GRIPPER'}) smach.StateMachine.add( 'CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'VERIFY_GRIPPER_CLOSED'}) 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': 'SET_NAMED_CONFIG_LIFT_OBJECT', 'timeout': 'CLOSE_GRIPPER', 'failure': 'CLOSE_GRIPPER' }) smach.StateMachine.add('SET_NAMED_CONFIG_LIFT_OBJECT', gbs.set_named_config('lift_object'), transitions={ 'success': 'PLAN_LIFT_OBJECT', 'failure': 'failure', 'timeout': 'SET_NAMED_CONFIG_LIFT_OBJECT' }) smach.StateMachine.add('PLAN_LIFT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[ ('/poses_to_move_wrtbase/event_in', 'e_start') ], event_out_list=[ ('/poses_to_move_wrtbase/event_out', 'e_success', True) ], timeout_duration=3), transitions={ 'success': 'LIFT_OBJECT', 'timeout': 'LIFT_OBJECT', 'failure': 'LIFT_OBJECT' }) smach.StateMachine.add( 'LIFT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in', 'e_start')], event_out_list=[('/cartesian_controller_demo/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'VERIFY_GRASPED', 'timeout': 'VERIFY_GRASPED', 'failure': 'VERIFY_GRASPED' }) smach.StateMachine.add( 'VERIFY_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=10), transitions={ 'success': 'SET_OBJECT_GRASPED', 'timeout': 'SET_OBJECT_NOT_GRASPED', 'failure': 'SET_OBJECT_NOT_GRASPED' }) smach.StateMachine.add('SET_OBJECT_GRASPED', mfs.set_is_object_grasped(True), transitions={'success': 'success'}) smach.StateMachine.add('SET_OBJECT_NOT_GRASPED', mfs.set_is_object_grasped(False), transitions={'success': 'success'})
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('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()
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=["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 __init__(self, use_mockup=None): smach.StateMachine.__init__(self, outcomes=['success', 'failure']) with self: #Opening the gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded':'STOP_POSE_SHIFTER'}) # Sending STOP to all components smach.StateMachine.add('STOP_POSE_SHIFTER', gbs.send_event([('/plate_pose_shifter/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_POSE_SHIFT'}) smach.StateMachine.add('SET_NAMED_CONFIG_POSE_SHIFT', gbs.set_named_config('plate_pose_shifter'), transitions={'success':'SHIFT_PLATE_POSE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_POSE_SHIFT'}) smach.StateMachine.add('SHIFT_PLATE_POSE', gbs.send_and_wait_events_combined( event_in_list=[('/plate_pose_shifter/event_in','e_start')], event_out_list=[('/plate_pose_shifter/event_out','e_success', True)], timeout_duration=5), transitions={'success':'SET_NAMED_CONFIG_PREGRASP', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('conveyor_belt_plate_pickup'), transitions={'success':'PLAN_ARM_MOTION', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_PREGRASP'}) 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)]), transitions={'success':'STOP_PLAN_ARM_MOTION', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in','e_stop')]), transitions={'success':'MOVE_ARM_TO_OBJECT'}) 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=10), transitions={'success':'STOP_MOVE_ARM_TO_OBJECT', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_APPROACH_PLATE'}) smach.StateMachine.add('SET_NAMED_CONFIG_APPROACH_PLATE', gbs.set_named_config('approach_plate'), transitions={'success':'PLAN_APPROACH_PLATE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_APPROACH_PLATE'}) smach.StateMachine.add('PLAN_APPROACH_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')], event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)], timeout_duration=10), transitions={'success':'APPROACH_PLATE', 'timeout':'APPROACH_PLATE', 'failure':'APPROACH_PLATE'}) smach.StateMachine.add('APPROACH_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in','e_start')], event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)], timeout_duration=10), transitions={'success':'CLOSE_GRIPPER', 'timeout':'CLOSE_GRIPPER', 'failure':'CLOSE_GRIPPER'}) smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded':'VERIFY_GRIPPER_CLOSED'}) 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':'STOP_APPROACH_PLATE', 'timeout':'CLOSE_GRIPPER', 'failure':'CLOSE_GRIPPER'}) smach.StateMachine.add('STOP_APPROACH_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'), ('/poses_to_move_wrtgripper/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_RETREAT_PLATE'}) smach.StateMachine.add('SET_NAMED_CONFIG_RETREAT_PLATE', gbs.set_named_config('retreat_plate'), transitions={'success':'PLAN_RETREAT_PLATE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_RETREAT_PLATE'}) smach.StateMachine.add('PLAN_RETREAT_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')], event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)], timeout_duration=10), transitions={'success':'RETREAT_PLATE', 'timeout':'RETREAT_PLATE', 'failure':'RETREAT_PLATE'}) smach.StateMachine.add('RETREAT_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in','e_start')], event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)], timeout_duration=10), transitions={'success':'STOP_RETREAT_PLATE', 'timeout':'STOP_RETREAT_PLATE', 'failure':'STOP_RETREAT_PLATE'}) smach.StateMachine.add('STOP_RETREAT_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'), ('/poses_to_move_wrtgripper/event_in','e_stop')]), transitions={'success':'MOVE_TO_HOLD'}) smach.StateMachine.add('MOVE_TO_HOLD', gms.move_arm('look_at_workspace'), transitions={'succeeded': 'success', 'failed': 'failure'})
def __init__(self, use_mockup=None): smach.StateMachine.__init__(self, outcomes=["success", "failure"]) with self: # Opening the gripper smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "STOP_POSE_SHIFTER"}, ) # Sending STOP to all components smach.StateMachine.add( "STOP_POSE_SHIFTER", gbs.send_event([("/plate_pose_shifter/event_in", "e_stop")]), transitions={"success": "SET_NAMED_CONFIG_POSE_SHIFT"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_POSE_SHIFT", gbs.set_named_config("plate_pose_shifter"), transitions={ "success": "SHIFT_PLATE_POSE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_POSE_SHIFT", }, ) smach.StateMachine.add( "SHIFT_PLATE_POSE", gbs.send_and_wait_events_combined( event_in_list=[("/plate_pose_shifter/event_in", "e_start") ], event_out_list=[("/plate_pose_shifter/event_out", "e_success", True)], timeout_duration=5, ), transitions={ "success": "SET_NAMED_CONFIG_PREGRASP", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "SET_NAMED_CONFIG_PREGRASP", gbs.set_named_config("conveyor_belt_plate_pickup"), transitions={ "success": "PLAN_ARM_MOTION", "failure": "failure", "timeout": "SET_NAMED_CONFIG_PREGRASP", }, ) 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)], ), transitions={ "success": "STOP_PLAN_ARM_MOTION", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_PLAN_ARM_MOTION", gbs.send_event([("/pregrasp_planner_pipeline/event_in", "e_stop")]), transitions={"success": "MOVE_ARM_TO_OBJECT"}, ) 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=10, ), transitions={ "success": "STOP_MOVE_ARM_TO_OBJECT", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_MOVE_ARM_TO_OBJECT", gbs.send_event([("/move_arm_planned/event_in", "e_stop")]), transitions={"success": "SET_NAMED_CONFIG_APPROACH_PLATE"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_APPROACH_PLATE", gbs.set_named_config("approach_plate"), transitions={ "success": "PLAN_APPROACH_PLATE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_APPROACH_PLATE", }, ) smach.StateMachine.add( "PLAN_APPROACH_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/poses_to_move_wrtgripper/event_in", "e_start")], event_out_list=[("/poses_to_move_wrtgripper/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "APPROACH_PLATE", "timeout": "APPROACH_PLATE", "failure": "APPROACH_PLATE", }, ) smach.StateMachine.add( "APPROACH_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/cartesian_controller_demo/event_in", "e_start")], event_out_list=[("/cartesian_controller_demo/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "CLOSE_GRIPPER", "timeout": "CLOSE_GRIPPER", "failure": "CLOSE_GRIPPER", }, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "VERIFY_GRIPPER_CLOSED"}, ) 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": "STOP_APPROACH_PLATE", "timeout": "CLOSE_GRIPPER", "failure": "CLOSE_GRIPPER", }, ) smach.StateMachine.add( "STOP_APPROACH_PLATE", gbs.send_event([ ("/cartesian_controller_demo/event_in", "e_stop"), ("/poses_to_move_wrtgripper/event_in", "e_stop"), ]), transitions={"success": "SET_NAMED_CONFIG_RETREAT_PLATE"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_RETREAT_PLATE", gbs.set_named_config("retreat_plate"), transitions={ "success": "PLAN_RETREAT_PLATE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_RETREAT_PLATE", }, ) smach.StateMachine.add( "PLAN_RETREAT_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/poses_to_move_wrtgripper/event_in", "e_start")], event_out_list=[("/poses_to_move_wrtgripper/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "RETREAT_PLATE", "timeout": "RETREAT_PLATE", "failure": "RETREAT_PLATE", }, ) smach.StateMachine.add( "RETREAT_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/cartesian_controller_demo/event_in", "e_start")], event_out_list=[("/cartesian_controller_demo/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "STOP_RETREAT_PLATE", "timeout": "STOP_RETREAT_PLATE", "failure": "STOP_RETREAT_PLATE", }, ) smach.StateMachine.add( "STOP_RETREAT_PLATE", gbs.send_event([ ("/cartesian_controller_demo/event_in", "e_stop"), ("/poses_to_move_wrtgripper/event_in", "e_stop"), ]), transitions={"success": "MOVE_TO_HOLD"}, ) smach.StateMachine.add( "MOVE_TO_HOLD", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "success", "failed": "failure" }, )
def main(): # Open the container rospy.init_node('pick_object_wbc_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': 'OPEN_GRIPPER', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'SET_DBC_PARAMS'}) smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'), transitions={ 'success': 'MOVE_ROBOT_AND_PICK', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) # whole body control command. It moves direct base controller and # calls pre-grasp planner, and (optionally) moves arm to object pose smach.StateMachine.add('MOVE_ROBOT_AND_PICK', 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=50), transitions={ 'success': 'CLOSE_GRIPPER', 'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE' }) smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', gbs.send_event([ ('/waypoint_trajectory_generation/event_in', 'e_start'), ('/wbc/event_in', 'e_stop') ]), transitions={'success': 'OVERALL_FAILED'}) smach.StateMachine.add( 'CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'}) # move arm to stage_intemediate position smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE', gms.move_arm('stage_intermediate'), transitions={ 'succeeded': 'VERIFY_OBJECT_GRASPED', 'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE' }) 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': 'OVERALL_SUCCESS', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = 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=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 __init__(self, use_mockup=None): smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached', 'no_more_free_poses', 'no_more_free_poses_at_robot_platf', 'no_more_task_for_given_type'], input_keys=['base_pose_to_approach', 'desired_distance_to_workspace', 'found_objects', 'lasttask', 'move_arm_to', 'move_base_by', 'next_arm_pose_index', 'object_pose', 'object_to_be_adjust_to', 'object_to_grasp', 'objects_to_be_grasped', 'prev_vs_result', 'rear_platform_free_poses', 'rear_platform_occupied_poses', 'recognized_objects', 'source_visits', 'task_list', 'test', 'vscount'], output_keys=['base_pose_to_approach', 'found_objects', 'lasttask', 'move_arm_to', 'move_base_by', 'next_arm_pose_index', 'object_to_be_adjust_to', 'object_to_grasp', 'objects_to_be_grasped', 'prev_vs_result', 'rear_platform_free_poses', 'rear_platform_occupied_poses', 'source_visits', 'task_list', 'test', 'vscount']) self.use_mockup = use_mockup self.use_visual_servoing = True with self: smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"), transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION_SAFE', 'no_more_task_for_given_type': 'no_more_task_for_given_type'}) # required before any call to gns.approach_pose, moves arm within footprint smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION_SAFE', gms.move_arm('look_at_workspace'), transitions={'succeeded': 'MOVE_TO_SOURCE_LOCATION', 'failed': 'MOVE_TO_SOURCE_LOCATION_SAFE'}) smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(), transitions={'succeeded': 'STOP_ALL_COMPONENTS_AT_START', 'failed': 'MOVE_TO_SOURCE_LOCATION'}) smach.StateMachine.add('STOP_ALL_COMPONENTS_AT_START', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'), ('/mcr_common/relative_displacement_calculator/event_in','e_stop'), ('/pregrasp_planner/event_in','e_stop'), ('/mcr_navigation/relative_base_controller/event_in','e_stop'), ('/planned_motion/event_in','e_stop')]), transitions={'success':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'}) smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.2), transitions={'succeeded':'SELECT_NEXT_LOOK_POSE', 'failed':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'}) smach.StateMachine.add('SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace_right', 'look_at_workspace','look_at_workspace_left']), transitions={'succeeded': 'LOOK_AROUND', 'failed': 'RECOGNIZE_OBJECTS'}) 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': 'ACCUMULATE_RECOGNIZED_LISTS', 'no_objects_found':'ACCUMULATE_RECOGNIZED_LISTS'}, remapping={'found_objects':'recognized_objects'}) smach.StateMachine.add('ACCUMULATE_RECOGNIZED_LISTS', gps.accumulate_recognized_objects_list(), transitions={'complete': 'SELECT_OBJECT_TO_BE_GRASPED', 'merged':'SELECT_NEXT_LOOK_POSE'}) smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(), transitions={'obj_selected':'COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', 'no_obj_selected':'SKIP_SOURCE_POSE', 'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'}) smach.StateMachine.add('COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/pregrasp_planner/event_in','e_start'), ('/gripper_to_object_pose_error_calculator/event_in','e_start'), ('/mcr_common/relative_displacement_calculator/event_in','e_start')]), transitions={'success':'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT'}) smach.StateMachine.add('WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.wait_for_events([('/pregrasp_planner/event_out','e_success', True), ('/gripper_to_object_pose_error_calculator/event_out','e_success', True), ('/mcr_common/relative_displacement_calculator/event_out','e_done', True)]), transitions={'success':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', 'timeout': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', 'failure':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE'}) smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'), ('/pregrasp_planner/event_in','e_stop'), ('/mcr_common/relative_displacement_calculator/event_in','e_stop')]), transitions={'success':'MOVE_ARM_AND_BASE_TO_OBJECT'}) smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'), ('/pregrasp_planner/event_in','e_stop'), ('/mcr_common/relative_displacement_calculator/event_in','e_stop')]), transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'}) smach.StateMachine.add('MOVE_ARM_AND_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_start'), ('/planned_motion/event_in', 'e_start')]), transitions={'success':'WAIT_ARM_AND_BASE_TO_OBJECT'}) smach.StateMachine.add('WAIT_ARM_AND_BASE_TO_OBJECT', gbs.wait_for_events([('/mcr_navigation/relative_base_controller/event_out','e_done', True), ('/mcr_navigation/collision_velocity_filter/event_out','e_zero_velocities_forwarded', False), ('/planned_motion/event_out', 'e_success', True)]), transitions={'success':'STOP_MOVE_ARM_BASE_TO_OBJECT', 'timeout': 'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', 'failure':'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE'}) smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'), ('/planned_motion/event_in','e_stop')]), # transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing transitions={'success':'GRASP_OBJ'}) # comment out if using visual servoing smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'), ('/planned_motion/event_in','e_stop')]), transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'}) smach.StateMachine.add('ALIGN_WITH_OBJECT_LOOP', mir_gbs.loop_for_vs(1), transitions={'loop': 'START_VISUAL_SERVOING', 'continue': 'GRASP_OBJ'}) smach.StateMachine.add('START_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_start')]), transitions={'success':'WAIT_VISUAL_SERVOING'}) smach.StateMachine.add('WAIT_VISUAL_SERVOING', gbs.wait_for_events([('/mcr_monitoring/component_wise_pose_error_monitor/event_out','e_stop', True)], timeout_duration=30), transitions={'success':'STOP_VISUAL_SERVOING', 'timeout': 'STOP_VISUAL_SERVOING_WITH_FAILURE', 'failure':'STOP_VISUAL_SERVOING_WITH_FAILURE'}) smach.StateMachine.add('STOP_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]), transitions={'success':'SET_VISUAL_SERVOING_SUCCESS'}) smach.StateMachine.add('STOP_VISUAL_SERVOING_WITH_FAILURE', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]), transitions={'success':'SET_VISUAL_SERVOING_FAILURE'}) smach.StateMachine.add('SET_VISUAL_SERVOING_SUCCESS', mir_gbs.set_vs_status(status=True), transitions={'success':'OPEN_GRIPPER_FOR_APPROACH_OBJECT'}) smach.StateMachine.add('SET_VISUAL_SERVOING_FAILURE', mir_gbs.set_vs_status(status=False), transitions={'success':'SELECT_OBJECT_TO_BE_GRASPED'}) smach.StateMachine.add('OPEN_GRIPPER_FOR_APPROACH_OBJECT', gms.control_gripper('open'), transitions={'succeeded':'APPROACH_OBJECT'}) smach.StateMachine.add('APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_start')]), transitions={'success':'WAIT_APPROACH_OBJECT'}) smach.StateMachine.add('WAIT_APPROACH_OBJECT', gbs.wait_for_events([('/guarded_approach_pose/coordinator/event_out','e_success', True)], timeout_duration=6), transitions={'success':'STOP_APPROACH_OBJECT', 'timeout': 'STOP_APPROACH_OBJECT', 'failure':'STOP_APPROACH_OBJECT'}) smach.StateMachine.add('STOP_APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_stop')]), transitions={'success':'CLOSE_GRIPPER'}) smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM'}) smach.StateMachine.add('GRASP_OBJ', gms.linear_motion(operation='grasp'), transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM', 'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop smach.StateMachine.add('GRASP_OBJ_VISUAL_SERVOING', gms.linear_motion(operation='grasp', offset_x=-0.05), transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM', 'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop smach.StateMachine.add('PRE_PLACE_OBJ_ON_REAR_PLATFORM', btts.pre_place_obj_on_rear_platform_btt(), transitions={'succeeded':'VERIFY_GRASPED', 'no_more_free_poses':'no_more_free_poses'}) smach.StateMachine.add('VERIFY_GRASPED', gbs.send_event([('/gripper_controller/grasp_monitor/event_in', 'e_trigger')]), transitions={'success':'WAIT_FOR_VERIFY_GRASPED'}) smach.StateMachine.add('WAIT_FOR_VERIFY_GRASPED', gbs.wait_for_events([('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5), transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM', 'timeout':'OPEN_GRIPPER_FOR_GRASP_FAILURE', 'failure':'OPEN_GRIPPER_FOR_GRASP_FAILURE'}) smach.StateMachine.add('OPEN_GRIPPER_FOR_GRASP_FAILURE', gms.control_gripper('open'), transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # THESE ARE NEVER ENTERED? # It would mean we've grasped the object - and don't have somewhere to place it. # If we don't have somewhere to place it, we won't grasp in the first place? smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(), transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS', 'no_more_free_poses':'no_more_free_poses'}) smach.StateMachine.add('DELETE_FROM_RECOGNIZED_OBJECTS', btts.delete_from_recognized_objects(), transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'}, remapping={'object_to_delete': 'object_to_grasp'}) smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_NEXT', gns.adjust_to_workspace(0.1), transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED', 'failed':'ADJUST_POSE_WRT_WORKSPACE_NEXT'}) smach.StateMachine.add('NO_MORE_FREE_POSES_SAFE',gms.move_arm('look_at_workspace'), transitions={'succeeded':'no_more_free_poses', 'failed':'SKIP_SOURCE_POSE_SAFE'}) # MISC STATES smach.StateMachine.add('SKIP_SOURCE_POSE_SAFE', gms.move_arm('look_at_workspace'), transitions={'succeeded':'SKIP_SOURCE_POSE', 'failed':'SKIP_SOURCE_POSE_SAFE'}) smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'), transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK', 'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
def __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'failed', 'no_object_for_ppt_platform'], input_keys=[ 'all_found_holes', 'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to', 'move_base_by', 'object_pose', 'rear_platform_free_poses', 'rear_platform_occupied_poses', 'selected_hole', 'selected_hole_pose', 'task_list' ], output_keys=[ 'all_found_holes', 'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to', 'move_base_by', 'rear_platform_free_poses', 'rear_platform_occupied_poses', 'selected_hole', 'selected_hole_pose', 'task_list' ]) with self: smach.StateMachine.add( 'ADD_WALLS_TO_PLANNING_SCENE', gms.configure_planning_scene("walls", "add"), transitions={'succeeded': 'MOVE_ARM_OUT_OF_VIEW'}) smach.StateMachine.add('MOVE_ARM_OUT_OF_VIEW', gms.move_arm('out_of_view'), transitions={ 'succeeded': 'FIND_HOLES', 'failed': 'MOVE_ARM_OUT_OF_VIEW' }) smach.StateMachine.add( 'FIND_HOLES', gps.find_holes(), transitions={ 'found_holes': 'SELECT_HOLE_TO_ADJUST_TO', 'found_no_holes': 'failed', # perform a BTT placement 'timeout': 'failed' }) smach.StateMachine.add( 'SELECT_HOLE_TO_ADJUST_TO', ppts.select_hole(), transitions={ 'hole_selected': 'COMPUTE_BASE_SHIFT_TO_OBJECT', 'no_match': 'failed', # perform a BTT placement 'no_more_obj_for_this_workspace': 'no_object_for_ppt_platform' }) smach.StateMachine.add( 'COMPUTE_BASE_SHIFT_TO_OBJECT', btts.compute_base_shift_to_object(), transitions={ 'succeeded': 'MOVE_BASE_RELATIVE', 'tf_error': 'COMPUTE_BASE_SHIFT_TO_OBJECT' }, remapping={'object_pose': 'selected_hole'} ) # TODO: can we do such a remapping, if not, we need a separate "selected_hole_pose" item in the userdata smach.StateMachine.add('MOVE_BASE_RELATIVE', gns.move_base_relative(), transitions={ 'succeeded': 'GRASP_OBJECT_FOR_HOLE_FROM_PLTF', 'timeout': 'MOVE_BASE_RELATIVE' }) smach.StateMachine.add('GRASP_OBJECT_FOR_HOLE_FROM_PLTF', ppts.grasp_obj_for_hole_from_pltf(), transitions={ 'object_grasped': 'MOVE_TO_INTERMEDIATE_POSE', 'no_more_obj_for_this_workspace': 'no_object_for_ppt_platform' }) ###### DO WE NEED THIS !?!?! BECAUSE OTHERWISE IT WILL HIT THE CAMERA AND ALIGN THE OBJECT "PROPERLY" !?!??! smach.StateMachine.add('MOVE_TO_INTERMEDIATE_POSE', gms.move_arm('platform_intermediate'), transitions={ 'succeeded': 'SELECT_ARM_POSITION', 'failed': 'MOVE_TO_INTERMEDIATE_POSE' }) smach.StateMachine.add( 'SELECT_ARM_POSITION', ppts.select_arm_position(), transitions={'arm_pose_selected': 'MOVE_ARM'}) smach.StateMachine.add('MOVE_ARM', gms.move_arm(), transitions={ 'succeeded': 'MOVE_GRIPPER', 'failed': 'MOVE_ARM' }) smach.StateMachine.add( 'MOVE_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'MOVE_ARM_TO_INTERMEDIATE_2'}) smach.StateMachine.add('MOVE_ARM_TO_INTERMEDIATE_2', gms.move_arm('platform_intermediate'), transitions={ 'succeeded': 'succeeded', 'failed': 'MOVE_ARM_TO_INTERMEDIATE_2' })
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':'OVERALL_FAILED', 'failure':'OVERALL_FAILED'}) 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'}) # 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(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("calibrate_pick_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) with sm: # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add( "STOP_REFERENCE_FRAME_PUB", gbs.send_event([("/static_transform_publisher_node/event_in", "e_stop")]), transitions={"success": "OPEN_GRIPPER"}, ) smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "MOVE_ARM"}, ) smach.StateMachine.add('MOVE_ARM', gms.move_arm('look_at_workspace_from_near'), transitions={ 'succeeded': 'PUBLISH_REFERENCE_FRAME', 'failed': 'MOVE_ARM' }) # 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": "WAIT_FOR_EVENT_FROM_USER"}, ) smach.StateMachine.add('WAIT_FOR_EVENT_FROM_USER', WaitForEventFromUser(), transitions={ 'success': 'MOVE_ROBOT_AND_PICK', 'failure': 'OVERALL_FAILED' }) # whole body control command. It moves direct base controller and # calls pre-grasp planner, and (optionally) moves arm to object pose smach.StateMachine.add( "MOVE_ROBOT_AND_PICK", 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=50, ), transitions={ "success": "OVERALL_SUCCESS", "timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", "failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", gbs.send_event([ ("/waypoint_trajectory_generation/event_in", "e_start"), ("/wbc/event_in", "e_stop"), ]), transitions={"success": "OVERALL_FAILED"}, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = IntrospectionServer("calibrate_pick_smach_viewer", sm, "/CALIBRATE_PICK_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="calibrate_pick_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()
def main(): rospy.init_node('stage_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['stage_object_goal'], output_keys = ['stage_object_feedback', 'stage_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', '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(), transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'}) """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('stage_object_smach_viewer', sm, '/STAGE_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'stage_object_server', action_spec = StageObjectAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'stage_object_goal', feedback_key = 'stage_object_feedback', result_key = 'stage_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("unstage_object_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) # read heavy object list sm.userdata.heavy_objects = rospy.get_param("~heavy_objects", ["m20_100"]) with sm: # add states to the container smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE", gms.move_arm("stage_intermediate"), transitions={ "succeeded": "CHECK_IF_OBJECT_HEAVY", "failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE", }, ) smach.StateMachine.add( "CHECK_IF_OBJECT_HEAVY", IsObjectHeavy(), transitions={ "heavy": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2", "light": "SETUP_MOVE_ARM_PRE_STAGE", }, ) smach.StateMachine.add( "MOVE_ARM_TO_STAGE_INTERMEDIATE_2", gms.move_arm("stage_intermediate_2"), transitions={ "succeeded": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY", "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": "CLOSE_GRIPPER", "failed": "MOVE_ARM_STAGE" }, ) smach.StateMachine.add( "SETUP_MOVE_ARM_PRE_STAGE_HEAVY", SetupMoveArm("pre", is_heavy=True), transitions={ "succeeded": "MOVE_ARM_PRE_STAGE_HEAVY", "failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY", }, ) smach.StateMachine.add( "MOVE_ARM_PRE_STAGE_HEAVY", gms.move_arm(), transitions={ "succeeded": "SETUP_MOVE_ARM_STAGE_HEAVY", "failed": "MOVE_ARM_PRE_STAGE_HEAVY", }, ) smach.StateMachine.add( "SETUP_MOVE_ARM_STAGE_HEAVY", SetupMoveArm("final", is_heavy=True), transitions={ "succeeded": "MOVE_ARM_STAGE_HEAVY", "failed": "SETUP_MOVE_ARM_STAGE_HEAVY", }, ) smach.StateMachine.add( "MOVE_ARM_STAGE_HEAVY", gms.move_arm(), transitions={ "succeeded": "CLOSE_GRIPPER", "failed": "MOVE_ARM_STAGE_HEAVY" }, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "CHECK_IF_OBJECT_HEAVY_AGAIN"}, ) # TODO: verify if object is grasped or not smach.StateMachine.add( "CHECK_IF_OBJECT_HEAVY_AGAIN", IsObjectHeavy(), transitions={ "heavy": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN", "light": "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=True), transitions={ "succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL", "failed": "MOVE_ARM_PRE_STAGE_AGAIN", }, ) smach.StateMachine.add( "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN", SetupMoveArm("pre", is_heavy=True), transitions={ "succeeded": "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN", "failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN", }, ) smach.StateMachine.add( "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN", gms.move_arm(blocking=False), transitions={ "succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL", "failed": "MOVE_ARM_PRE_STAGE_HEAVY_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_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("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()
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 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 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(): 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()