def main(): rospy.init_node('perceive_location_server') sleep_time = rospy.get_param('~sleep_time', 1.0) # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['perceive_location_goal'], output_keys=['perceive_location_feedback', 'perceive_location_result']) # Open the container with sm: # approach to platform smach.StateMachine.add( 'GET_GOAL', getGoal(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'}) #transitions={'succeeded':'START_OBJECT_LIST_MERGER'}) # generates a pose based on the previous string object topic received smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'SUBSCRIBE_TO_POINT_CLOUD'}) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'START_OBJECT_LIST_MERGER'}) smach.StateMachine.add( 'START_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/object_list_merger/event_in', 'e_start'), ('/mcr_perception/object_selector/event_in', 'e_start') ], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'LOOK_AT_WORKSPACE', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'), smach.StateMachine.add('LOOK_AT_WORKSPACE', gms.move_arm('look_at_workspace_from_far'), transitions={ 'succeeded': 'START_SEGMENTATION', 'failed': 'LOOK_AT_WORKSPACE' }) # this is new scene segmentation pipeline combined with object detection smach.StateMachine.add( 'START_SEGMENTATION', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/scene_segmentation/event_in', 'e_start')], event_out_list=[ ('/mcr_perception/scene_segmentation/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'WAIT_LEFT', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) smach.StateMachine.add('WAIT_LEFT', wait_for(sleep_time=sleep_time), transitions={'success': 'ADD_POINT_CLOUD'}) smach.StateMachine.add( 'ADD_POINT_CLOUD', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/scene_segmentation/event_in', 'e_add_cloud_start')], event_out_list=[ ('/mcr_perception/scene_segmentation/event_out', 'e_add_cloud_stopped', True) ], timeout_duration=5), transitions={ 'success': 'RECOGNIZE_OBJECTS', 'timeout': 'ADD_POINT_CLOUD', 'failure': 'SET_ACTION_LIB_FAILURE' }) # in the new scene_segmentation pipeline, object detector can be called by string msg e_segment smach.StateMachine.add( 'RECOGNIZE_OBJECTS', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_perception/multimodal_object_identification/input/event_in', 'e_trigger'), ('/mcr_perception/scene_segmentation/event_in', 'e_segment')], event_out_list= [('/mcr_perception/scene_segmentation/event_out', 'e_done', True), ('/mcr_perception/multimodal_object_identification/output/event_out', 'e_done', True)], timeout_duration=7), transitions={ 'success': 'STOP_PUBLISH_ACCUMULATED_PC', 'timeout': 'STOP_PUBLISH_ACCUMULATED_PC', 'failure': 'STOP_PUBLISH_ACCUMULATED_PC' }) smach.StateMachine.add( 'STOP_PUBLISH_ACCUMULATED_PC', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/scene_segmentation/event_in', 'e_stop')], event_out_list=[ ('/mcr_perception/scene_segmentation/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'STOP_COMPONENTS', 'timeout': 'STOP_COMPONENTS', 'failure': 'STOP_COMPONENTS' }) smach.StateMachine.add( 'STOP_COMPONENTS', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/object_list_merger/event_in', 'e_stop'), ('/mcr_perception/scene_segmentation/event_in', 'e_stop') ], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_stopped', True), ('/mcr_perception/scene_segmentation/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD' }) smach.StateMachine.add( 'UNSUBSCRIBE_FROM_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/empty_topic')]), transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'}) smach.StateMachine.add( 'PUBLISH_MERGED_OBJECT_LIST', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_trigger')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_done', True) ], timeout_duration=5), transitions={ 'success': 'SET_ACTION_LIB_SUCCESS', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_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 viewer sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm, '/PERCEIVE_LOCATION_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_location_server', action_spec=PerceiveLocationAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='perceive_location_goal', feedback_key='perceive_location_feedback', result_key='perceive_location_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node("move_base_safe_server") # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED", "OVERALL_PREEMPTED"], input_keys=["goal"], output_keys=["feedback", "result"], ) with sm: smach.StateMachine.add( "CHECK_IF_BARRIER_TAPE_ENABLED", CheckDontBeSafe(), transitions={ "safe": "START_BARRIER_TAPE_DETECTION", "unsafe": "SETUP_MOVE_BASE", }, ) smach.StateMachine.add( "START_BARRIER_TAPE_DETECTION", gbs.send_event([( "/mir_perception/barrier_tape_detection/event_in", "e_start", )]), transitions={"success": "MOVE_ARM_TO_DETECT_BARRIER_TAPE"}, ) smach.StateMachine.add( "MOVE_ARM_TO_DETECT_BARRIER_TAPE", gms.move_arm("barrier_tape", blocking=False), transitions={ "succeeded": "SETUP_MOVE_BASE", "failed": "MOVE_ARM_TO_DETECT_BARRIER_TAPE" }, ) # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add( "SETUP_MOVE_BASE", SetupMoveBase("/move_base_wrapper/pose_in"), transitions={ "succeeded": "SET_DIRECT_BASE_CONTROLLER_PARAMETERS", "failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "SET_DIRECT_BASE_CONTROLLER_PARAMETERS", gbs.set_named_config("dbc_move_base"), transitions={ "success": "START_MOVE_BASE", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "START_MOVE_BASE", StartMoveBase( event_in_topic="/move_base_wrapper/event_in", event_out_topic="/move_base_wrapper/event_out", timeout_duration=50, ), transitions={ "success": "PREPARE_ARM_FOR_NEXT_ACTION", "timeout": "RESET_BARRIER_TAPE", "failure": "RESET_BARRIER_TAPE", "stopped": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "RESET_BARRIER_TAPE", gbs.send_event([( "/mir_perception/barrier_tape_detection/event_in", "e_reset", ), ( "/move_base_wrapper/event_in", "e_stop", )]), transitions={"success": "SETUP_MOVE_BASE_AGAIN"}, ) smach.StateMachine.add( "SETUP_MOVE_BASE_AGAIN", SetupMoveBase("/move_base_wrapper/pose_in"), transitions={ "succeeded": "START_MOVE_BASE_AGAIN", "failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "preempted": "OVERALL_PREEMPTED", }, ) smach.StateMachine.add( "START_MOVE_BASE_AGAIN", gbs.send_and_wait_events_combined( event_in_list=[("/move_base_wrapper/event_in", "e_start")], event_out_list=[("/move_base_wrapper/event_out", "e_success", True)], timeout_duration=50, ), transitions={ "success": "PREPARE_ARM_FOR_NEXT_ACTION", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) # send arm to a position depending on next action smach.StateMachine.add( "PREPARE_ARM_FOR_NEXT_ACTION", PrepareArmForNextAction(), transitions={ "succeeded": "MOVE_ARM_FOR_NEXT_ACTION", "skipped": "REACH_DESTINATION_PRECISELY", "failed": "PREPARE_ARM_FOR_NEXT_ACTION", }, ) smach.StateMachine.add( "MOVE_ARM_FOR_NEXT_ACTION", gms.move_arm(blocking=False), transitions={ "succeeded": "REACH_DESTINATION_PRECISELY", "failed": "REACH_DESTINATION_PRECISELY" }, ) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add( "REACH_DESTINATION_PRECISELY", gbs.send_and_wait_events_combined( event_in_list=[ ( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_start", ), ], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_success", True, )], timeout_duration=5, ), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={ "success": "STOP_CONTROLLER_WITH_SUCCESS", "timeout": "STOP_CONTROLLER_WITH_SUCCESS", "failure": "STOP_CONTROLLER_WITH_FAILURE", }, ) # stop controller with success smach.StateMachine.add( "STOP_CONTROLLER_WITH_SUCCESS", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_stop", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_stopped", True, )], timeout_duration=1, ), transitions={ "success": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) # stop controller with failure smach.StateMachine.add( "STOP_CONTROLLER_WITH_FAILURE", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_stop", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_stopped", True, )], timeout_duration=1, ), transitions={ "success": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", "failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", }, ) smach.StateMachine.add( "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE", gbs.send_event([("/mir_perception/barrier_tape_detection/event_in", "e_stop"), ( "/move_base_wrapper/event_in", "e_stop", )]), transitions={"success": "OVERALL_FAILED"}, ) smach.StateMachine.add( "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS", gbs.send_event([("/mir_perception/barrier_tape_detection/event_in", "e_stop")]), transitions={"success": "ALIGN_WITH_WORKSPACE"}, ) smach.StateMachine.add( "ALIGN_WITH_WORKSPACE", AlignWithWorkspace(), transitions={ "succeeded": "OVERALL_SUCCESS", "failed": "OVERALL_SUCCESS", }, ) state_publisher = rospy.Publisher('~current_state', String, queue_size=1) sm.register_transition_cb(transition_cb, [state_publisher]) sm.register_start_cb(start_cb, [state_publisher]) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = smach_ros.IntrospectionServer("move_base_safe_smach_viewer", sm, "/MOVE_BASE_SAFE_SMACH_VIEWER") sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="move_base_safe_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["OVERALL_PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('perceive_location_server') sleep_time = rospy.get_param('~sleep_time', 1.0) # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['goal'], output_keys=['feedback', 'result']) # Open the container sm.userdata.arm_pose_list = [ 'look_at_workspace_from_near', 'look_at_workspace_from_near_left', 'look_at_workspace_from_near_right' ] sm.userdata.arm_pose_index = 0 base_x_offset = rospy.get_param('~base_x_offset', 0.0) base_y_offset = rospy.get_param('~base_y_offset', 0.25) base_theta_offset = rospy.get_param('~base_theta_offset', 0.0) sm.userdata.base_pose_list = [{ 'x': 0.0, 'y': 0.0, 'theta': 0.0 }, { 'x': base_x_offset, 'y': base_y_offset, 'theta': base_theta_offset }, { 'x': base_x_offset, 'y': -base_y_offset, 'theta': -base_theta_offset }] sm.userdata.base_pose_index = 0 with sm: # approach to platform smach.StateMachine.add( 'SETUP', Setup(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'}) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'SET_DBC_PARAMS'}) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'), transitions={ 'success': 'START_OBJECT_LIST_MERGER', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'START_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/object_list_merger/event_in', 'e_start'), ('/mcr_perception/object_selector/event_in', 'e_start') ], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'GET_MOTION_TYPE', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add('GET_MOTION_TYPE', GetMotionType(), transitions={ 'base_motion': 'SET_APPROPRIATE_BASE_POSE', 'arm_motion': 'SET_APPROPRIATE_ARM_POSE' }) smach.StateMachine.add('SET_APPROPRIATE_BASE_POSE', SetupMoveBaseWithDBC(), transitions={ 'pose_set': 'MOVE_BASE_WITH_DBC', 'tried_all': 'POPULATE_RESULT_WITH_OBJECTS' }) smach.StateMachine.add( 'MOVE_BASE_WITH_DBC', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'MOVE_ARM', 'timeout': 'STOP_DBC', 'failure': 'STOP_DBC' }) smach.StateMachine.add('SET_APPROPRIATE_ARM_POSE', SetupMoveArm(), transitions={ 'pose_set': 'MOVE_ARM', 'tried_all': 'POPULATE_RESULT_WITH_OBJECTS' }) # move arm to appropriate position smach.StateMachine.add('MOVE_ARM', gms.move_arm_and_gripper('open'), transitions={ 'succeeded': 'START_OBJECT_RECOGNITION', 'failed': 'MOVE_ARM' }) # New perception pipeline state machine smach.StateMachine.add( 'START_OBJECT_RECOGNITION', gbs.send_and_wait_events_combined( event_in_list=[ ('/mir_perception/multimodal_object_recognition/event_in', 'e_start') ], event_out_list=[ ('/mir_perception/multimodal_object_recognition/event_out', 'e_done', True) ], timeout_duration=10), transitions={ 'success': 'STOP_RECOGNITION', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'STOP_RECOGNITION', gbs.send_and_wait_events_combined( event_in_list=[ ('/mir_perception/multimodal_object_recognition/event_in', 'e_stop') ], event_out_list=[ ('/mir_perception/multimodal_object_recognition/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'STOP_OBJECT_LIST_MERGER', 'timeout': 'STOP_OBJECT_LIST_MERGER', 'failure': 'STOP_OBJECT_LIST_MERGER' }) smach.StateMachine.add( 'STOP_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_stop')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'PUBLISH_MERGED_OBJECT_LIST', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) smach.StateMachine.add( 'PUBLISH_MERGED_OBJECT_LIST', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_trigger')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_done', True) ], timeout_duration=5), transitions={ 'success': 'CHECK_IF_OBJECTS_FOUND', 'timeout': 'CHECK_IF_OBJECTS_FOUND', 'failure': 'CHECK_IF_OBJECTS_FOUND' }) smach.StateMachine.add( 'CHECK_IF_OBJECTS_FOUND', gbs.send_and_wait_events_combined( event_in_list=[( '/mcr_perception/object_list_merger/object_found_event_in', 'e_trigger')], event_out_list=[( '/mcr_perception/object_list_merger/object_found_event_out', 'e_objects_found', True)], timeout_duration=10.0), transitions={ 'success': 'POPULATE_RESULT_WITH_OBJECTS', 'timeout': 'OVERALL_FAILED', 'failure': 'GET_MOTION_TYPE' }) # populate action server result with perceived objects smach.StateMachine.add('POPULATE_RESULT_WITH_OBJECTS', PopulateResultWithObjects(), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add( 'STOP_DBC', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=10), transitions={ 'success': 'OVERALL_FAILED', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = IntrospectionServer('perceive_location_smach_viewer', sm, '/PERCEIVE_LOCATION_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_location_server', action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='goal', feedback_key='feedback', result_key='result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('perceive_location_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['perceive_location_goal'], output_keys=['perceive_location_feedback', 'perceive_location_result']) # Open the container with sm: # approach to platform smach.StateMachine.add( 'GET_GOAL', getGoal(), transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'}) #transitions={'succeeded':'START_OBJECT_LIST_MERGER'}) # generates a pose based on the previous string object topic received smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'START_OBJECT_LIST_MERGER'}) smach.StateMachine.add( 'START_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_start')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'LOOK_AT_WORKSPACE_LEFT', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'), smach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_left'), transitions={ 'succeeded': 'SUBSCRIBE_TO_POINT_CLOUD', 'failed': 'LOOK_AT_WORKSPACE_LEFT' }) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'RECOGNIZE_OBJECTS_LEFT'}) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_LEFT', gps.find_objects(retries=1), transitions={ 'objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT', 'no_objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT' }, #transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_2', # 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_2'}, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD_2', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'LOOK_AT_WORKSPACE_STRAIGHT'}) # send arm to a position in which the depth camera can see the platform smach.StateMachine.add('LOOK_AT_WORKSPACE_STRAIGHT', gms.move_arm('look_at_workspace'), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS_STRAIGHT', 'failed': 'LOOK_AT_WORKSPACE_STRAIGHT' }) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_STRAIGHT', gps.find_objects(retries=1), transitions={ 'objects_found': 'LOOK_AT_WORKSPACE_RIGHT', 'no_objects_found': 'LOOK_AT_WORKSPACE_RIGHT' }, #transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_3', # 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_3'}, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'SUBSCRIBE_TO_POINT_CLOUD_3', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/arm_cam3d/depth_registered/points')]), transitions={'success': 'LOOK_AT_WORKSPACE_RIGHT'}) # send arm to a position in which the depth camera can see the platform smach.StateMachine.add('LOOK_AT_WORKSPACE_RIGHT', gms.move_arm('look_at_workspace_right'), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS_RIGHT', 'failed': 'LOOK_AT_WORKSPACE_RIGHT' }) # trigger perception pipeline smach.StateMachine.add( 'RECOGNIZE_OBJECTS_RIGHT', gps.find_objects(retries=1), transitions={ 'objects_found': 'STOP_OBJECT_LIST_MERGER', 'no_objects_found': 'STOP_OBJECT_LIST_MERGER' }, remapping={'found_objects': 'recognized_objects'}) smach.StateMachine.add( 'STOP_OBJECT_LIST_MERGER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_stop')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_stopped', True) ], timeout_duration=5), transitions={ 'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD', 'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD' }) smach.StateMachine.add( 'UNSUBSCRIBE_FROM_POINT_CLOUD', gbs.send_event([('/mcr_perception/mux_pointcloud/select', '/emtpty_topic')]), transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'}) smach.StateMachine.add( 'PUBLISH_MERGED_OBJECT_LIST', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_perception/object_list_merger/event_in', 'e_trigger')], event_out_list=[ ('/mcr_perception/object_list_merger/event_out', 'e_done', True) ], timeout_duration=5), transitions={ 'success': 'SET_ACTION_LIB_SUCCESS', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # smach viewer sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm, '/PERCEIVE_LOCATION_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='perceive_location_server', action_spec=PerceiveLocationAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='perceive_location_goal', feedback_key='perceive_location_feedback', result_key='perceive_location_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('compute_base_shift_server') # Construct state machine sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'], input_keys=['compute_base_shift_goal'], output_keys=[ 'compute_base_shift_feedback', 'compute_base_shift_result' ]) sm.userdata.next_arm_pose_index = 0 sm.userdata.move_arm_to = None with sm: #add states to the container # remove accumulated base poses and start non loop dependant components smach.StateMachine.add( 'CLEAR_BASE_POSES', gbs.send_event([ ('/move_base_relative/pose_selector/event_in', 'e_forget'), ('/move_base_relative/random_obj_selector_transformer_republisher/event_in', 'e_start'), ('/move_base_relative/modified_pose_transformer/event_in', 'e_start') ]), transitions={'success': 'SELECT_NEXT_LOOK_POSE'}) # select a look_at_workspace pose smach.StateMachine.add( 'SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace']), transitions={ 'succeeded': 'LOOK_AROUND', # next pose selected 'completed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS', 'failed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS' } ) # we've run out of poses to select, which means we've gone through the list # move arm to selected pose smach.StateMachine.add('LOOK_AROUND', gms.move_arm(), transitions={ 'succeeded': 'RECOGNIZE_OBJECTS', 'failed': 'LOOK_AROUND' }) smach.StateMachine.add( 'RECOGNIZE_OBJECTS', gps.find_objects(retries=1), transitions={ 'objects_found': 'SELECT_OBJECT', 'no_objects_found': 'SELECT_NEXT_LOOK_POSE' }, remapping={'found_objects': 'recognized_objects'}) # here the object selector is set up to select objects randomly # it will publish object name (goes to pose selector) and object pose (goes to compute base shift pipeline) # relative displacement calculator pose republisher in odom frame is also triggered here, before doing computations smach.StateMachine.add( 'SELECT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/random_object_selector/event_in', 'e_trigger') ], event_out_list=[ ('/mcr_perception/random_object_selector/event_out', 'e_selected', True) ], timeout_duration=10), transitions={ 'success': 'SELECT_OBJECT', 'timeout': 'SELECT_NEXT_LOOK_POSE', 'failure': 'SELECT_NEXT_LOOK_POSE' } ) # this means we're done computing poses for all recognized objects from this pose # stop relative displacement calculator pose republisher and after that go to success smach.StateMachine.add( 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS', gbs.send_event([ ('/move_base_relative/random_obj_selector_transformer_republisher/event_in', 'e_stop'), ('/move_base_relative/modified_pose_transformer/event_in', 'e_stop') ]), transitions={'success': 'SET_ACTION_LIB_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper(server_name='compute_base_shift_server', action_spec=ComputeBaseShiftAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['PREEMPTED'], goal_key='compute_base_shift_goal', feedback_key='compute_base_shift_feedback', result_key='compute_base_shift_result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node("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(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(): # 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 __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'failed', 'timeout'], input_keys=[], output_keys=[]) with self: # Move robot #smach.StateMachine.add('SHIFT_BASE_1', ShiftBase([0.0, 0.05, 0.0]), #transitions={'succeeded': 'failed', # 'timeout': 'failed', # 'failed': 'failed'}) # start point could accumulator smach.StateMachine.add( 'START_ACCUMULATOR', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/cloud_accumulator/event_in', 'e_start') ], event_out_list=[ ('/mcr_perception/cloud_accumulator/event_out', 'e_started', True) ], timeout_duration=5), transitions={ 'success': 'ADD_POINT_COULD_1', 'timeout': 'timeout', 'failure': 'failed' }) # add point could smach.StateMachine.add( 'ADD_POINT_COULD_1', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/cloud_accumulator/event_in', 'e_add_cloud_start') ], event_out_list=[ ('/mcr_perception/cloud_accumulator/event_out', 'e_add_cloud_stopped', True) ], timeout_duration=5), transitions={ 'success': 'SHIFT_BASE', 'timeout': 'ADD_POINT_COULD_1', 'failure': 'failed' }) # Move robot smach.StateMachine.add('SHIFT_BASE', ShiftBase([0.0, 0.30, 0.0]), transitions={ 'succeeded': 'ADD_POINT_COULD_2', 'timeout': 'ADD_POINT_COULD_2', 'failed': 'ADD_POINT_COULD_2' }) # add point could smach.StateMachine.add( 'ADD_POINT_COULD_2', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/cloud_accumulator/event_in', 'e_add_cloud_start') ], event_out_list=[ ('/mcr_perception/cloud_accumulator/event_out', 'e_add_cloud_stopped', True) ], timeout_duration=5), transitions={ 'success': 'SHIFT_BASE_BACK', 'timeout': 'ADD_POINT_COULD_2', 'failure': 'failed' }) # Move robot smach.StateMachine.add('SHIFT_BASE_BACK', ShiftBase([0.0, -0.30, 0.0]), transitions={ 'succeeded': 'succeeded', 'timeout': 'succeeded', 'failed': 'succeeded' })
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"], ) # read large object list sm.userdata.large_objects = rospy.get_param("~large_objects", ["S40_40_B", "S40_40_G", "M30", "BEARING_BOX", "MOTOR"]) sm.userdata.reperceive = rospy.get_param("~reperceive", True) 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_PICKING", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) # whole body control command. It moves direct base controller and # checks if an IK soln exists for the arm. smach.StateMachine.add( "MOVE_ROBOT_AND_TRY_PICKING", 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": "CHECK_IF_REPERCEIVE", "timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", "failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "CHECK_IF_REPERCEIVE", ShouldReperceive(), transitions={ "yes": "OPEN_GRIPPER_FOR_REPERCEIVE", "no": "GENERATE_OBJECT_POSE_AGAIN", }, ) smach.StateMachine.add( "OPEN_GRIPPER_FOR_REPERCEIVE", gms.control_gripper("open"), transitions={"succeeded": "MOVE_ARM"}, ) # move arm to appropriate position smach.StateMachine.add( "MOVE_ARM", gms.move_arm("look_at_workspace_from_near"), transitions={ "succeeded": "START_OBJECT_LIST_MERGER", "failed": "MOVE_ARM", }, ) smach.StateMachine.add( "START_OBJECT_LIST_MERGER", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_start")], event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_started", True)], timeout_duration=5, ), transitions={ "success": "WAIT_FOR_ARM_TO_STABILIZE", "timeout": "TRY_PICKING", "failure": "TRY_PICKING", }, ) smach.StateMachine.add( "WAIT_FOR_ARM_TO_STABILIZE", mir_gbs.wait_for(0.5), transitions={ "succeeded": "START_OBJECT_RECOGNITION", }, ) # New perception pipeline state machine smach.StateMachine.add( "START_OBJECT_RECOGNITION", gbs.send_and_wait_events_combined( event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_start")], event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_done", True)], timeout_duration=10, ), transitions={ "success": "STOP_RECOGNITION", "timeout": "TRY_PICKING", "failure": "TRY_PICKING", }, ) smach.StateMachine.add( "STOP_RECOGNITION", gbs.send_and_wait_events_combined( event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_stop")], event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_stopped", True)], timeout_duration=5, ), transitions={ "success": "STOP_OBJECT_LIST_MERGER", "timeout": "TRY_PICKING", "failure": "TRY_PICKING", }, ) smach.StateMachine.add( "STOP_OBJECT_LIST_MERGER", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_stop")], event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_stopped", True)], timeout_duration=5, ), transitions={ "success": "PUBLISH_MERGED_OBJECT_LIST", "timeout": "TRY_PICKING", "failure": "TRY_PICKING", }, ) smach.StateMachine.add( "PUBLISH_MERGED_OBJECT_LIST", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_trigger_local")], event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_done", True)], timeout_duration=5, ), transitions={ "success": "SELECT_OBJECT_AGAIN", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "SELECT_OBJECT_AGAIN", SelectObject("/mcr_perception/local_object_selector/input/object_name"), transitions={"succeeded": "GENERATE_UPDATED_OBJECT_POSE"}, ) # generates a pose of object smach.StateMachine.add( "GENERATE_UPDATED_OBJECT_POSE", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/local_object_selector/event_in", "e_trigger")], event_out_list=[("/mcr_perception/local_object_selector/event_out", "e_selected", True)], timeout_duration=10, ), transitions={ "success": "CHECK_IF_OBJECT_LARGE_LOCAL", "timeout": "GENERATE_OBJECT_POSE_AGAIN", "failure": "GENERATE_OBJECT_POSE_AGAIN", }, ) # generates a pose of object smach.StateMachine.add( "GENERATE_OBJECT_POSE_AGAIN", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_selector/event_in", "e_re_trigger")], event_out_list=[("/mcr_perception/object_selector/event_out", "e_selected", True)], timeout_duration=10, ), transitions={ "success": "CHECK_IF_OBJECT_LARGE", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "CHECK_IF_OBJECT_LARGE_LOCAL", IsObjectLarge(), transitions={ "large": "OPEN_GRIPPER_WIDE_LOCAL", "small": "OPEN_GRIPPER_NARROW_LOCAL", }, ) smach.StateMachine.add( "OPEN_GRIPPER_WIDE_LOCAL", gms.control_gripper("open"), transitions={"succeeded": "TRY_PICKING"}, ) smach.StateMachine.add( "OPEN_GRIPPER_NARROW_LOCAL", gms.control_gripper("open_narrow"), transitions={"succeeded": "TRY_PICKING"}, ) # move only arm for wbc smach.StateMachine.add( "TRY_PICKING", 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": "CLOSE_GRIPPER", "timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", "failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE", }, ) smach.StateMachine.add( "CHECK_IF_OBJECT_LARGE", IsObjectLarge(), transitions={ "large": "OPEN_GRIPPER_WIDE", "small": "OPEN_GRIPPER_NARROW", }, ) smach.StateMachine.add( "OPEN_GRIPPER_WIDE", gms.control_gripper("open"), transitions={"succeeded": "MOVE_ROBOT_AND_PICK"}, ) smach.StateMachine.add( "OPEN_GRIPPER_NARROW", gms.control_gripper("open_narrow"), transitions={"succeeded": "MOVE_ROBOT_AND_PICK"}, ) # 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_stop"), ("/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", }, ) sm.register_transition_cb(transition_cb) sm.register_start_cb(start_cb) # 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 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: # open gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'SETUP_MOVE_ARM'}) #add states to the container smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(), transitions={'success': 'MOVE_ARM'}) smach.StateMachine.add('MOVE_ARM', gms.move_arm(), transitions={'succeeded': 'WAIT_FOR_EVENT_FROM_USER', 'failed': 'MOVE_ARM'}) smach.StateMachine.add('WAIT_FOR_EVENT_FROM_USER', WaitForEventFromUser(), transitions={'success' : 'PLAN_WHOLE_BODY_MOTION', 'failure' : 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_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':'STOP_PLAN_WHOLE_BODY_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('STOP_PLAN_WHOLE_BODY_MOTION', 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 ########################################## 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':'SET_ACTION_LIB_SUCCESS'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','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':'MOVE_ARM_TO_HOLD_FAILURE'}) 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('calibrate_pick_object_smach_viewer', sm, '/CALIBRATE_PICK_OBJECT_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name = 'calibrate_pick_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(mokeup=False): # Open the container rospy.init_node('insert_object_in_cavity_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: # publish object as string to mcr_perception_selectors -> cavity, this component then publishes # pose in base_link reference frame smach.StateMachine.add( 'SELECT_OBJECT', select_object('/mcr_perception/cavity_pose_selector/object_name'), transitions={'success': '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': '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 ################################################## smach.StateMachine.add( 'SELECT_PREGRASP_PLANNER_INPUT_2', gbs.send_event([ ('/mir_manipulation/mux_pregrasp_pose/select', '/mcr_perception/object_selector/output/object_pose') ]), transitions={'success': 'SELECT_OBJECT_2'}) smach.StateMachine.add( 'SELECT_OBJECT_2', select_object('/mcr_perception/cavity_pose_selector/object_name'), transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE_2'}) smach.StateMachine.add( 'CHECK_IF_OBJECT_IS_AVAILABLE_2', 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': 'PLAN_ARM_MOTION_2', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) smach.StateMachine.add( 'PLAN_ARM_MOTION_2', gbs.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', gbs.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=[('/move_arm_planned/event_in','e_start')], #event_out_list=[('/move_arm_planned/event_out','e_success', True)], 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', gbs.send_event([('/waypoint_trajectory_generation/event_out', 'e_stop')]), transitions={'success': 'OPEN_GRIPPER'}) #smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECTH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'), ################################ When failure place the object smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', gbs.send_event([('/waypoint_trajectory_generation/event_out', 'e_stop')]), transitions={'success': 'MOVE_ARM_TO_DEFAULT_PLACE'}) 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'), transitions={'succeeded': 'MOVE_ARM_BACK'}) smach.StateMachine.add('MOVE_ARM_BACK', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'SET_ACTION_LIB_FAILURE', 'failed': 'SET_ACTION_LIB_FAILURE' }) ################################ # close gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded': 'WIGGLE_ARM_LEFT'}) ################################### # wiggling the arm for precision placement smach.StateMachine.add('WIGGLE_ARM_LEFT', ppt_wiggle_arm(wiggle_offset=-0.12, joint=0), transitions={ 'succeeded': 'WIGGLE_ARM_RIGHT', 'failed': 'WIGGLE_ARM_RIGHT' }) smach.StateMachine.add('WIGGLE_ARM_RIGHT', ppt_wiggle_arm(wiggle_offset=0.24, joint=0), transitions={ 'succeeded': 'WIGGLE_ARM_FORWARD', 'failed': 'WIGGLE_ARM_FORWARD' }) # Now moving perpendicular smach.StateMachine.add('WIGGLE_ARM_FORWARD', ppt_wiggle_arm(wiggle_offset=-0.12, joint=3), transitions={ 'succeeded': 'WIGGLE_ARM_BACKWARD', 'failed': 'WIGGLE_ARM_BACKWARD' }) smach.StateMachine.add('WIGGLE_ARM_BACKWARD', ppt_wiggle_arm(wiggle_offset=0.24, joint=3), 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': 'SET_ACTION_LIB_SUCCESS', 'failed': 'MOVE_ARM_TO_HOLD' }) # set action lib result smach.StateMachine.add( 'SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded': 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS'}) smach.StateMachine.add( 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS', send_event('/mir_manipulation/mux_pregrasp_pose/select', '/mcr_perception/object_selector/output/object_pose'), transitions={'success': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add( 'SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'MOVE_ARM_TO_HOLD_FAILURE'}) smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'OVERALL_FAILED', 'failed': 'MOVE_ARM_TO_HOLD_FAILURE' }) # smach viewer sis = smach_ros.IntrospectionServer( 'insert_object_smach_viewer', sm, '/INSERT_OBJECT_IN_CAVITY_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='insert_object_in_cavity_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_wheel_in_axis_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: smach.StateMachine.add( 'PUBLISH_REFERENCE_FRAME', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), transitions={'success': 'GET_AXIS_POSE'}) smach.StateMachine.add( 'GET_AXIS_POSE', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/car_wheel_axis_detector/input/event_in', 'e_trigger') ], event_out_list=[( '/mcr_perception/car_wheel_axis_detector/output/event_out', 'e_done', True)], timeout_duration=10), transitions={ 'success': 'SHIFT_AXIS_POSE', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) smach.StateMachine.add('SHIFT_AXIS_POSE', gbs.send_and_wait_events_combined( event_in_list=[ ('/wheel_axis_pose_shifter/event_in', 'e_start') ], event_out_list=[ ('/wheel_axis_pose_shifter/event_out', 'e_success', True) ], timeout_duration=5), 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': 'PLAN_ARM_MOTION' }) # 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': 'SETUP_MOVE_ARM_PRE_STAGE'}) ############################################### unstage start ######################################## 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_TO_GRASP', 'failed': 'MOVE_ARM_PRE_STAGE' }) smach.StateMachine.add( 'OPEN_GRIPPER_TO_GRASP', gms.control_gripper('open'), 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': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'}) 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_HOLD_1', 'failed': 'MOVE_ARM_PRE_STAGE_AGAIN' }) smach.StateMachine.add('MOVE_ARM_TO_HOLD_1', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'MOVE_ROBOT_TO_OBJECT', 'failed': 'MOVE_ARM_TO_HOLD_1' }) ############################################### unstage end ######################################## # 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', gbs.send_event([( '/wbc/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')]), transitions={'success': 'PLAN_ARM_MOTION'}) # send stop event_in to arm motion component and return failure smach.StateMachine.add( 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', gbs.send_event([( '/wbc/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')]), transitions={'success': 'PLAN_ARM_MOTION'}) ########################################## PREGRASP PIPELINE 2 ################################################## 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=15), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', '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', gbs.send_event([ ('/pregrasp_planner_pipeline/event_in', 'e_stop') ]), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) # execute robot motion smach.StateMachine.add( 'MOVE_ARM_TO_OBJECT', 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', gbs.send_event([('/waypoint_trajectory_generation/event_out', 'e_stop')]), transitions={'success': 'STOP_LINEAR_COMPONENTS_1'}) #smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECTH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'), smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', gbs.send_event([('/waypoint_trajectory_generation/event_out', 'e_stop')]), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) ########################################## LINEAR MOTION PIPELINE ################################################## smach.StateMachine.add('STOP_LINEAR_COMPONENTS_1', gbs.send_event([ ('/poses_to_move/event_in', 'e_stop'), ('/cartesian_controller_demo/event_in', 'e_stop') ]), transitions={'success': 'PLAN_LINEAR_APPROACH'}) smach.StateMachine.add('PLAN_LINEAR_APPROACH', gbs.send_and_wait_events_combined( event_in_list=[('/poses_to_move/event_in', 'e_start')], event_out_list=[('/poses_to_move/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'MOVE_ARM_LINEAR', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) smach.StateMachine.add('MOVE_ARM_LINEAR', 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_LINEAR_COMPONENTS_2', 'timeout': 'STOP_LINEAR_COMPONENTS_2', 'failure': 'STOP_LINEAR_COMPONENTS_2' }) smach.StateMachine.add('STOP_LINEAR_COMPONENTS_2', gbs.send_event([ ('/poses_to_move/event_in', 'e_stop'), ('/cartesian_controller_demo/event_in', 'e_stop') ]), transitions={'success': 'OPEN_GRIPPER'}) ########################################## LINEAR MOTION PIPELINE END ################################################## # close gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), 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' }) # 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': 'MOVE_ARM_TO_HOLD_FAILURE'}) smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'OVERALL_FAILED', 'failed': 'MOVE_ARM_TO_HOLD_FAILURE' }) # smach viewer sis = smach_ros.IntrospectionServer('insert_wheel_smach_viewer', sm, '/INSERT_WHEEL_IN_AXIS_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='insert_wheel_in_axis_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("planning_coordinator") problem_file = rospy.get_param("~problem_file", None) domain_file = rospy.get_param("~domain_file", None) planner = rospy.get_param("~planner", "mercury") if problem_file is None or domain_file is None: rospy.logfatal( "Either domain and/or problem file not provided. Exiting.") sys.exit(1) # Create a SMACH state machine sm = smach.StateMachine( outcomes=["OVERALL_FAILURE", "OVERALL_PREEMPT", "OVERALL_SUCCESS"]) sm.userdata.already_once_executed = False # counter for how many times did it enter check_execution_already_started() sm.userdata.check_execution_counter = 0 sm.userdata.domain_file = domain_file sm.userdata.problem_file = problem_file sm.userdata.planner = planner # Open the container with sm: # upload intrinsic knowledge smach.StateMachine.add( "UPLOAD_INTRINSIC_KNOWLEDGE", gbs.send_event([("/upload_knowledge/event_in", "e_trigger")]), transitions={"success": "LOOK_FOR_UNFINISHED_GOALS"}, ) # TODO:wait for upload intrinsic knowledge succes response # send even in to look for goals component to process and see if there are unfinished goals on the knowledge base smach.StateMachine.add( "LOOK_FOR_UNFINISHED_GOALS", gbs.send_and_wait_events_combined( event_in_list=[ ("/knowledge_base_analyzer/pending_goals/event_in", "e_start") ], event_out_list=[( "/knowledge_base_analyzer/pending_goals/event_out", "e_goals_available", True, )], timeout_duration=5, ), transitions={ "success": "GENERATE_PDDL_PROBLEM", "timeout": "CHECK_EXECUTION_ALREADY_STARTED", "failure": "CHECK_EXECUTION_ALREADY_STARTED", }, ) smach.StateMachine.add( "CHECK_EXECUTION_ALREADY_STARTED", check_execution_already_started(max_count=3), transitions={ "success": "MOVE_TO_EXIT", "failure": "RE_ADD_GOALS" }, ) smach.StateMachine.add( "RE_ADD_GOALS", re_add_goals(), transitions={ "success": "LOOK_FOR_UNFINISHED_GOALS", "failure": "LOOK_FOR_UNFINISHED_GOALS", }, ) # generate problem from knowledge base snapshot smach.StateMachine.add( "GENERATE_PDDL_PROBLEM", gbs.send_and_wait_events_combined( event_in_list=[ ("/mir_planning/pddl_problem_generator/event_in", "e_trigger") ], event_out_list=[( "/mir_planning/pddl_problem_generator/event_out", "e_success", True, )], timeout_duration=5, ), transitions={ "success": "PLAN", "timeout": "GENERATE_PDDL_PROBLEM", "failure": "OVERALL_FAILURE", }, ) # plan task (mode can be either PlanGoal.NORMAL or PlanGoal.FAST) smach.StateMachine.add( "PLAN", plan_task(mode=PlanGoal.NORMAL), transitions={ "success": "EXECUTE_PLAN", "failure": "LOOK_FOR_UNFINISHED_GOALS", }, ) smach.StateMachine.add( "EXECUTE_PLAN", execute_plan(), transitions={ "success": "LOOK_FOR_UNFINISHED_GOALS", "failure": "LOOK_FOR_UNFINISHED_GOALS", "preempted": "OVERALL_PREEMPT", }, ) smach.StateMachine.add( "MOVE_TO_EXIT", gas.move_base("EXIT"), transitions={ "success": "OVERALL_SUCCESS", "failed": "MOVE_TO_EXIT" }, ) # Create a thread to execute the smach container smach_thread = threading.Thread(target=sm.execute) smach_thread.start() while not rospy.is_shutdown() and smach_thread.isAlive(): rospy.sleep(1.0) if rospy.is_shutdown(): rospy.logwarn("ctrl + c detected!!! preempting smach execution") # Request the container to preempt sm.request_preempt() smach_thread.join()
def main(): rospy.init_node('align_with_object_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'], input_keys = ['align_with_object_goal'], output_keys = ['align_with_object_feedback', 'align_with_object_result']) with sm: #add states to the container # publish name of object to which we want to align smach.StateMachine.add('PUBLISH_OBJECT_NAME', select_object('/move_base_relative/pose_selector/selected_object_name'), transitions={'success':'SELECT_POSE_FOR_OBJECT'}) # trigger pose selector. this will use the object name to output the corresponding pose (in map/odom frame) smach.StateMachine.add('SELECT_POSE_FOR_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/move_base_relative/pose_selector/event_in','e_trigger')], event_out_list=[('/move_base_relative/pose_selector/event_out','e_selected', True)], timeout_duration=10), transitions={'success':'START_DIRECT_BASE_CONTROLLER', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) # object is not in the list # call direct base controller # Note: the pose out from pose selector needs to be mapped to input pose of direct base controller # time out of 20 seconds is set smach.StateMachine.add('START_DIRECT_BASE_CONTROLLER', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True), # if this happens it has succeeded ('/mcr_navigation/collision_velocity_filter/event_out', 'e_zero_velocities_forwarded', False)], # if this happens we stop timeout_duration=20), transitions={'success':'STOP_CONTROLLER_WITH_SUCCESS', 'timeout':'STOP_CONTROLLER_WITH_FAILURE', 'failure':'STOP_CONTROLLER_WITH_FAILURE'}) # this means we're done computing poses for all recognized objects from this pose # stop controller with success smach.StateMachine.add('STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)], timeout_duration=1), transitions={'success':'SET_ACTION_LIB_SUCCESS', 'timeout':'SET_ACTION_LIB_SUCCESS', 'failure':'SET_ACTION_LIB_FAILURE'}) # stop controller with failure smach.StateMachine.add('STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)], timeout_duration=1), transitions={'success':'SET_ACTION_LIB_FAILURE', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), transitions={'succeeded':'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper( server_name = 'align_with_object_server', action_spec = AlignWithObjectAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['PREEMPTED'], goal_key = 'align_with_object_goal', feedback_key = 'align_with_object_feedback', result_key = 'align_with_object_result') # Run the server in a background thread asw.run_server() rospy.spin()
def __init__(self, base_shift=[0.0, 0.0, 0.0]): smach.StateMachine.__init__( self, outcomes=['succeeded', 'failed', 'timeout'], input_keys=[], output_keys=[]) with self: # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add('SHIFT_BASE_COMMAND', ShiftBaseCommand(base_shift), transitions={'succeeded': 'MOVE_BASE'}) # send event_in to move base to a pose smach.StateMachine.add( 'MOVE_BASE', gbs.send_and_wait_events_combined( event_in_list=[ ('/mcr_perception/percieve_location_server/pose_transformer/event_in', 'e_start'), ], event_out_list= [('/mcr_perception/percieve_location_server/pose_transformer/event_out', 'e_success', True)], timeout_duration=50), transitions={ 'success': 'ADJUST_BASE', 'timeout': 'timeout', 'failure': 'failed' }) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add( 'ADJUST_BASE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True)], timeout_duration=5 ), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={ 'success': 'STOP_CONTROLLER_WITH_SUCCESS', 'timeout': 'STOP_CONTROLLER_WITH_SUCCESS', 'failure': 'STOP_CONTROLLER_WITH_FAILURE' }) # stop controller with success smach.StateMachine.add( 'STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'succeeded', 'timeout': 'succeeded', 'failure': 'failed' }) # stop controller with failure smach.StateMachine.add( 'STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'failed', 'timeout': 'failed', 'failure': 'failed' })
def main(): rospy.init_node('planning_coordinator') problem_file = rospy.get_param('~problem_file', None) domain_file = rospy.get_param('~domain_file', None) planner = rospy.get_param('~planner', 'mercury') if problem_file is None or domain_file is None: rospy.logfatal( 'Either domain and/or problem file not provided. Exiting.') sys.exit(1) # Create a SMACH state machine sm = smach.StateMachine( outcomes=['OVERALL_FAILURE', 'OVERALL_PREEMPT', 'OVERALL_SUCCESS']) sm.userdata.already_once_executed = False # counter for how many times did it enter check_execution_already_started() sm.userdata.check_execution_counter = 0 sm.userdata.domain_file = domain_file sm.userdata.problem_file = problem_file sm.userdata.planner = planner # Open the container with sm: # upload intrinsic knowledge smach.StateMachine.add( 'UPLOAD_INTRINSIC_KNOWLEDGE', gbs.send_event([('/upload_knowledge/event_in', 'e_trigger')]), transitions={'success': 'LOOK_FOR_UNFINISHED_GOALS'}) # TODO:wait for upload intrinsic knowledge succes response # send even in to look for goals component to process and see if there are unfinished goals on the knowledge base smach.StateMachine.add( 'LOOK_FOR_UNFINISHED_GOALS', gbs.send_and_wait_events_combined( event_in_list=[ ('/knowledge_base_analyzer/pending_goals/event_in', 'e_start') ], event_out_list=[ ('/knowledge_base_analyzer/pending_goals/event_out', 'e_goals_available', True) ], timeout_duration=5), transitions={ 'success': 'GENERATE_PDDL_PROBLEM', 'timeout': 'CHECK_EXECUTION_ALREADY_STARTED', 'failure': 'CHECK_EXECUTION_ALREADY_STARTED' }) smach.StateMachine.add('CHECK_EXECUTION_ALREADY_STARTED', check_execution_already_started(max_count=3), transitions={ 'success': 'MOVE_TO_EXIT', 'failure': 'RE_ADD_GOALS' }) smach.StateMachine.add('RE_ADD_GOALS', re_add_goals(), transitions={ 'success': 'LOOK_FOR_UNFINISHED_GOALS', 'failure': 'LOOK_FOR_UNFINISHED_GOALS' }) # generate problem from knowledge base snapshot smach.StateMachine.add( 'GENERATE_PDDL_PROBLEM', gbs.send_and_wait_events_combined( event_in_list=[ ('/task_planning/pddl_problem_generator_node/event_in', 'e_trigger') ], event_out_list=[ ('/task_planning/pddl_problem_generator_node/event_out', 'e_success', True) ], timeout_duration=5), transitions={ 'success': 'PLAN', 'timeout': 'GENERATE_PDDL_PROBLEM', 'failure': 'OVERALL_FAILURE' }) # plan task (mode can be either PlanGoal.NORMAL or PlanGoal.FAST) smach.StateMachine.add('PLAN', plan_task(mode=PlanGoal.NORMAL), transitions={ 'success': 'EXECUTE_PLAN', 'failure': 'LOOK_FOR_UNFINISHED_GOALS' }) smach.StateMachine.add('EXECUTE_PLAN', execute_plan(), transitions={ 'success': 'LOOK_FOR_UNFINISHED_GOALS', 'failure': 'LOOK_FOR_UNFINISHED_GOALS', 'preempted': 'OVERALL_PREEMPT' }) smach.StateMachine.add('MOVE_TO_EXIT', gas.move_base('EXIT'), transitions={ 'success': 'OVERALL_SUCCESS', 'failed': 'MOVE_TO_EXIT' }) # Create a thread to execute the smach container smach_thread = threading.Thread(target=sm.execute) smach_thread.start() while not rospy.is_shutdown() and smach_thread.isAlive(): rospy.sleep(1.0) if rospy.is_shutdown(): rospy.logwarn("ctrl + c detected!!! preempting smach execution") # Request the container to preempt sm.request_preempt() smach_thread.join()
def main(): rospy.init_node("perceive_location_server") sleep_time = rospy.get_param("~sleep_time", 1.0) # Construct state machine sm = smach.StateMachine( outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"], input_keys=["goal"], output_keys=["feedback", "result"], ) # Open the container sm.userdata.arm_pose_list = [ "look_at_workspace_from_near", "look_at_workspace_from_near_left", "look_at_workspace_from_near_right", ] sm.userdata.arm_pose_index = 0 base_x_offset = rospy.get_param("~base_x_offset", 0.0) base_y_offset = rospy.get_param("~base_y_offset", 0.25) base_theta_offset = rospy.get_param("~base_theta_offset", 0.0) sm.userdata.base_pose_list = [ { "x": 0.0, "y": 0.0, "theta": 0.0 }, { "x": base_x_offset, "y": base_y_offset, "theta": base_theta_offset }, { "x": base_x_offset, "y": -base_y_offset, "theta": -base_theta_offset }, ] sm.userdata.base_pose_index = 0 with sm: # approach to platform smach.StateMachine.add( "SETUP", Setup(), transitions={"succeeded": "PUBLISH_REFERENCE_FRAME"}, ) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add( "PUBLISH_REFERENCE_FRAME", gbs.send_event([("/static_transform_publisher_node/event_in", "e_start")]), transitions={"success": "SET_DBC_PARAMS"}, ) # publish a static frame which will be used as reference for perceived objs smach.StateMachine.add( "SET_DBC_PARAMS", gbs.set_named_config("dbc_pick_object"), transitions={ "success": "START_OBJECT_LIST_MERGER", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "START_OBJECT_LIST_MERGER", gbs.send_and_wait_events_combined( event_in_list=[ ("/mcr_perception/object_list_merger/event_in", "e_start"), ("/mcr_perception/object_selector/event_in", "e_start"), ], event_out_list=[( "/mcr_perception/object_list_merger/event_out", "e_started", True, )], timeout_duration=5, ), transitions={ "success": "GET_MOTION_TYPE", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "GET_MOTION_TYPE", GetMotionType(), transitions={ "base_motion": "SET_APPROPRIATE_BASE_POSE", "arm_motion": "SET_APPROPRIATE_ARM_POSE", }, ) smach.StateMachine.add( "SET_APPROPRIATE_BASE_POSE", SetupMoveBaseWithDBC(), transitions={ "pose_set": "MOVE_BASE_WITH_DBC", "tried_all": "POPULATE_RESULT_WITH_OBJECTS", }, ) smach.StateMachine.add( "MOVE_BASE_WITH_DBC", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_start", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_success", True, )], timeout_duration=10, ), transitions={ "success": "MOVE_ARM", "timeout": "STOP_DBC", "failure": "STOP_DBC", }, ) smach.StateMachine.add( "SET_APPROPRIATE_ARM_POSE", SetupMoveArm(), transitions={ "pose_set": "MOVE_ARM", "tried_all": "STOP_OBJECT_LIST_MERGER", }, ) # move arm to appropriate position smach.StateMachine.add( "MOVE_ARM", gms.move_arm_and_gripper("open"), transitions={ "succeeded": "WAIT_FOR_ARM_TO_STABILIZE", "failed": "MOVE_ARM", }, ) # move arm to appropriate position smach.StateMachine.add( "WAIT_FOR_ARM_TO_STABILIZE", mir_gbs.wait_for(0.5), transitions={ "succeeded": "START_OBJECT_RECOGNITION", }, ) # New perception pipeline state machine smach.StateMachine.add( "START_OBJECT_RECOGNITION", gbs.send_and_wait_events_combined( event_in_list=[( "/mir_perception/multimodal_object_recognition/event_in", "e_start", )], event_out_list=[( "/mir_perception/multimodal_object_recognition/event_out", "e_done", True, )], timeout_duration=10, ), transitions={ "success": "STOP_RECOGNITION", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "STOP_RECOGNITION", gbs.send_and_wait_events_combined( event_in_list=[( "/mir_perception/multimodal_object_recognition/event_in", "e_stop", )], event_out_list=[( "/mir_perception/multimodal_object_recognition/event_out", "e_stopped", True, )], timeout_duration=5, ), transitions={ "success": "GET_MOTION_TYPE", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "STOP_OBJECT_LIST_MERGER", gbs.send_and_wait_events_combined( event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_stop")], event_out_list=[( "/mcr_perception/object_list_merger/event_out", "e_stopped", True, )], timeout_duration=5, ), transitions={ "success": "PUBLISH_MERGED_OBJECT_LIST", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "PUBLISH_MERGED_OBJECT_LIST", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_perception/object_list_merger/event_in", "e_trigger", )], event_out_list=[( "/mcr_perception/object_list_merger/event_out", "e_done", True, )], timeout_duration=5, ), transitions={ "success": "CHECK_IF_OBJECTS_FOUND", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) smach.StateMachine.add( "CHECK_IF_OBJECTS_FOUND", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_perception/object_list_merger/object_found_event_in", "e_trigger", )], event_out_list=[( "/mcr_perception/object_list_merger/object_found_event_out", "e_objects_found", True, )], timeout_duration=5.0, ), transitions={ "success": "POPULATE_RESULT_WITH_OBJECTS", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) # populate action server result with perceived objects smach.StateMachine.add( "POPULATE_RESULT_WITH_OBJECTS", PopulateResultWithObjects(), transitions={"succeeded": "OVERALL_SUCCESS"}, ) smach.StateMachine.add( "STOP_DBC", gbs.send_and_wait_events_combined( event_in_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_in", "e_stop", )], event_out_list=[( "/mcr_navigation/direct_base_controller/coordinator/event_out", "e_stopped", True, )], timeout_duration=10, ), transitions={ "success": "OVERALL_FAILED", "timeout": "OVERALL_FAILED", "failure": "OVERALL_FAILED", }, ) # smach viewer if rospy.get_param("~viewer_enabled", False): sis = IntrospectionServer( "perceive_location_smach_viewer", sm, "/PERCEIVE_LOCATION_SMACH_VIEWER", ) sis.start() # Construct action server wrapper asw = ActionServerWrapper( server_name="perceive_location_server", action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=["OVERALL_SUCCESS"], aborted_outcomes=["OVERALL_FAILED"], preempted_outcomes=["PREEMPTED"], goal_key="goal", feedback_key="feedback", result_key="result", ) # Run the server in a background thread asw.run_server() rospy.spin()
def main(): rospy.init_node('move_base_safe_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'OVERALL_PREEMPTED'], input_keys=['move_base_safe_goal'], output_keys=['move_base_safe_feedback', 'move_base_safe_result']) with sm: #add states to the container # smach.StateMachine.add('USE_FAR_RANGE_CAMERA_FOR_BARRIER_TAPE', gbs.set_named_config('far_range_camera_barrier_tape'), # transitions={'success':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER', # 'failure':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER', # 'timeout':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER'}) # # smach.StateMachine.add('SUBSCRIBE_TO_POINT_CLOUD_BARRIER', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/arm_cam3d/depth_registered/points')]), # transitions={'success':'SETUP_MOVE_ARM'}) smach.StateMachine.add('CHECK_DONT_BE_SAFE', CheckDontBeSafe(), transitions={ 'safe': 'START_BARRIER_TAPE_DETECTION', 'unsafe': 'SETUP_MOVE_BASE' }) #smach.StateMachine.add('START_BARRIER_TAPE_DETECTION', gbs.send_event([('/mcr_perception/barrier_tape_detection/fake_event', 'e_trigger')]), smach.StateMachine.add( 'START_BARRIER_TAPE_DETECTION', gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in', 'e_trigger')]), transitions={'success': 'SETUP_MOVE_ARM'}) smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(), transitions={ 'succeeded': 'MOVE_ARM', 'failed': 'SETUP_MOVE_ARM' }) ''' smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_wide'), transitions={'succeeded': 'MOVE_ARM'}) smach.StateMachine.add('MOVE_ARM', gms.move_arm(), transitions={'succeeded': 'SETUP_MOVE_BASE', 'failed': 'MOVE_ARM'}) ''' smach.StateMachine.add('MOVE_ARM', gms.move_arm_and_gripper('open_wide'), transitions={ 'succeeded': 'SETUP_MOVE_BASE', 'failed': 'MOVE_ARM' }) # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add('SETUP_MOVE_BASE', SetupMoveBase('/move_base_wrapper/pose_in'), transitions={ 'succeeded': 'SET_DBC_PARAMS', 'failed': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'preempted': 'OVERALL_PREEMPTED' }) smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_move_base'), transitions={ 'success': 'MOVE_BASE', 'timeout': 'SET_ACTION_LIB_FAILURE', 'failure': 'SET_ACTION_LIB_FAILURE' }) # send event_in to move base to a pose # add events to mir_look at and mcr_converter smach.StateMachine.add( 'MOVE_BASE', gbs. send_and_wait_events_combined(event_in_list=[ ('/move_base_wrapper/event_in', 'e_start'), ('/mcr_common/twist_to_motion_direction_conversion/event_in', 'e_start'), ('/mir_manipulation/look_at_point/event_in', 'e_start') ], event_out_list=[ ('/move_base_wrapper/event_out', 'e_success', True) ], timeout_duration=50), transitions={ 'success': 'ADJUST_BASE', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add( 'ADJUST_BASE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start'), ('/mir_manipulation/look_at_point/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True)], timeout_duration=5 ), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={ 'success': 'STOP_CONTROLLER_WITH_SUCCESS', 'timeout': 'STOP_CONTROLLER_WITH_SUCCESS', 'failure': 'STOP_CONTROLLER_WITH_FAILURE' }) # stop controller with success smach.StateMachine.add( 'STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) # stop controller with failure smach.StateMachine.add( 'STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) smach.StateMachine.add( 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in', 'e_stop')]), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add( 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in', 'e_stop')]), transitions={'success': 'ALIGN_WITH_WORKSPACE'}) # smach.StateMachine.add('SET_ACTION_LIB_FAILURE_RESET_CAMERA', gbs.set_named_config('close_range_camera_perception'), # transitions={'success':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE', # 'failure':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE', # 'timeout':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE'}) # # smach.StateMachine.add('SET_ACTION_LIB_SUCCESS_RESET_CAMERA', gbs.set_named_config('close_range_camera_perception'), # transitions={'success':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS', # 'failure':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS', # 'timeout':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS'}) # # smach.StateMachine.add('UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/empty_topic')]), # transitions={'success':'SET_ACTION_LIB_SUCCESS'}) # # smach.StateMachine.add('UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/empty_topic')]), # transitions={'success':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add( 'ALIGN_WITH_WORKSPACE', AlignWithWorkspace(), #transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'}) transitions={ 'succeeded': 'SET_ACTION_LIB_SUCCESS', 'failed': 'SET_ACTION_LIB_SUCCESS' }) smach.StateMachine.add( 'SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), #transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'}) transitions={'succeeded': 'OVERALL_SUCCESS'}) # generates a pose based on the previous string object topic received #smach.StateMachine.add('RESET_STATIC_TRANSFORM_FOR_PERCEPTION', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), # transitions={'success':'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper(server_name='move_base_safe_server', action_spec=MoveBaseSafeAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['OVERALL_PREEMPTED'], goal_key='move_base_safe_goal', feedback_key='move_base_safe_feedback', result_key='move_base_safe_result') # Run the server in a background thread asw.run_server() rospy.spin()
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 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("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 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('move_base_safe_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'OVERALL_PREEMPTED'], input_keys=['goal'], output_keys=['feedback', 'result']) with sm: smach.StateMachine.add('CHECK_DONT_BE_SAFE', CheckDontBeSafe(), transitions={ 'safe': 'START_BARRIER_TAPE_DETECTION', 'unsafe': 'SETUP_MOVE_BASE' }) smach.StateMachine.add( 'START_BARRIER_TAPE_DETECTION', gbs.send_event([('/mir_perception/barrier_tape_detection/event_in', 'e_start')]), transitions={'success': 'MOVE_ARM'}) smach.StateMachine.add('MOVE_ARM', gms.move_arm('barrier_tape', blocking=False), transitions={ 'succeeded': 'SETUP_MOVE_BASE', 'failed': 'MOVE_ARM' }) # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add('SETUP_MOVE_BASE', SetupMoveBase('/move_base_wrapper/pose_in'), transitions={ 'succeeded': 'SET_DBC_PARAMS', 'failed': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'preempted': 'OVERALL_PREEMPTED' }) smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_move_base'), transitions={ 'success': 'MOVE_BASE', 'timeout': 'OVERALL_FAILED', 'failure': 'OVERALL_FAILED' }) # send event_in to move base to a pose # add events to mir_look at and mcr_converter smach.StateMachine.add( 'MOVE_BASE', gbs. send_and_wait_events_combined(event_in_list=[ ('/move_base_wrapper/event_in', 'e_start'), ('/mcr_common/twist_to_motion_direction_conversion/event_in', 'e_start'), ('/mir_manipulation/look_at_point/event_in', 'e_start') ], event_out_list=[ ('/move_base_wrapper/event_out', 'e_success', True) ], timeout_duration=50), transitions={ 'success': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE', 'timeout': 'RESET_BARRIER_TAPE', 'failure': 'RESET_BARRIER_TAPE' }) smach.StateMachine.add( 'RESET_BARRIER_TAPE', gbs.send_event([('/mir_perception/barrier_tape_detection/event_in', 'e_reset')]), transitions={'success': 'SETUP_MOVE_BASE_AGAIN'}) smach.StateMachine.add('SETUP_MOVE_BASE_AGAIN', SetupMoveBase('/move_base_wrapper/pose_in'), transitions={ 'succeeded': 'MOVE_BASE_AGAIN', 'failed': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'preempted': 'OVERALL_PREEMPTED' }) smach.StateMachine.add( 'MOVE_BASE_AGAIN', gbs. send_and_wait_events_combined(event_in_list=[ ('/move_base_wrapper/event_in', 'e_start'), ('/mcr_common/twist_to_motion_direction_conversion/event_in', 'e_start'), ('/mir_manipulation/look_at_point/event_in', 'e_start') ], event_out_list=[ ('/move_base_wrapper/event_out', 'e_success', True) ], timeout_duration=50), transitions={ 'success': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) # send arm to a position depending on next action smach.StateMachine.add('SETUP_MOVE_ARM_AFTER_MOVE_BASE', SetupMoveArmAfterMoveBase(), transitions={ 'succeeded': 'MOVE_ARM_AFTER_MOVE_BASE', 'skipped': 'ADJUST_BASE', 'failed': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE' }) smach.StateMachine.add('MOVE_ARM_AFTER_MOVE_BASE', gms.move_arm(blocking=False), transitions={ 'succeeded': 'ADJUST_BASE', 'failed': 'ADJUST_BASE' }) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add( 'ADJUST_BASE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start'), ('/mir_manipulation/look_at_point/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True)], timeout_duration=5 ), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={ 'success': 'STOP_CONTROLLER_WITH_SUCCESS', 'timeout': 'STOP_CONTROLLER_WITH_SUCCESS', 'failure': 'STOP_CONTROLLER_WITH_FAILURE' }) # stop controller with success smach.StateMachine.add( 'STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) # stop controller with failure smach.StateMachine.add( 'STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined( event_in_list= [('/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_stop')], event_out_list= [('/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_stopped', True)], timeout_duration=1), transitions={ 'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', 'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE' }) smach.StateMachine.add( 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE', gbs.send_event([('/mir_perception/barrier_tape_detection/event_in', 'e_stop')]), transitions={'success': 'OVERALL_FAILED'}) smach.StateMachine.add( 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS', gbs.send_event([('/mir_perception/barrier_tape_detection/event_in', 'e_stop')]), transitions={'success': 'ALIGN_WITH_WORKSPACE'}) smach.StateMachine.add('ALIGN_WITH_WORKSPACE', AlignWithWorkspace(), transitions={ 'succeeded': 'OVERALL_SUCCESS', 'failed': 'OVERALL_SUCCESS' }) # smach viewer if rospy.get_param('~viewer_enabled', False): sis = smach_ros.IntrospectionServer('move_base_safe_smach_viewer', sm, '/MOVE_BASE_SAFE_SMACH_VIEWER') sis.start() # Construct action server wrapper asw = ActionServerWrapper(server_name='move_base_safe_server', action_spec=GenericExecuteAction, wrapped_container=sm, succeeded_outcomes=['OVERALL_SUCCESS'], aborted_outcomes=['OVERALL_FAILED'], preempted_outcomes=['OVERALL_PREEMPTED'], goal_key='goal', feedback_key='feedback', result_key='result') # Run the server in a background thread asw.run_server() rospy.spin()
def main(): # 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(): # 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: # 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('move_base_safe_server') # Construct state machine sm = smach.StateMachine( outcomes=['OVERALL_SUCCESS','OVERALL_FAILED','OVERALL_PREEMPTED'], input_keys = ['move_base_safe_goal'], output_keys = ['move_base_safe_feedback', 'move_base_safe_result']) with sm: #add states to the container smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(), transitions={'succeeded': 'MOVE_ARM', 'failed': 'SETUP_MOVE_ARM'}) smach.StateMachine.add('MOVE_ARM', gms.move_arm(), transitions={'succeeded': 'SETUP_MOVE_BASE', 'failed': 'MOVE_ARM'}) # get pose from action lib as string, convert to pose stamped and publish smach.StateMachine.add('SETUP_MOVE_BASE', SetupMoveBase('/move_base_wrapper/pose_in'), transitions={'succeeded': 'MOVE_BASE', 'failed': 'SET_ACTION_LIB_FAILURE', 'preempted':'OVERALL_PREEMPTED'}) # send event_in to move base to a pose smach.StateMachine.add('MOVE_BASE', gbs.send_and_wait_events_combined( event_in_list=[('/move_base_wrapper/event_in','e_start')], event_out_list=[('/move_base_wrapper/event_out','e_success', True)], timeout_duration=50), transitions={'success':'ADJUST_BASE', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SETUP_MOVE_BASE'}) # call direct base controller to fine adjust the base to the final desired pose # (navigation tolerance is set to a wide tolerance) smach.StateMachine.add('ADJUST_BASE', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)], timeout_duration=5), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase transitions={'success':'STOP_CONTROLLER_WITH_SUCCESS', 'timeout':'STOP_CONTROLLER_WITH_SUCCESS', 'failure':'STOP_CONTROLLER_WITH_FAILURE'}) # stop controller with success smach.StateMachine.add('STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)], timeout_duration=1), transitions={'success':'SET_ACTION_LIB_SUCCESS', 'timeout':'SET_ACTION_LIB_SUCCESS', 'failure':'SET_ACTION_LIB_FAILURE'}) # stop controller with failure smach.StateMachine.add('STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined( event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')], event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)], timeout_duration=1), transitions={'success':'SET_ACTION_LIB_FAILURE', 'timeout':'SET_ACTION_LIB_FAILURE', 'failure':'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True), #transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'}) transitions={'succeeded':'OVERALL_SUCCESS'}) # generates a pose based on the previous string object topic received #smach.StateMachine.add('RESET_STATIC_TRANSFORM_FOR_PERCEPTION', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]), # transitions={'success':'OVERALL_SUCCESS'}) smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded':'OVERALL_FAILED'}) # Construct action server wrapper asw = ActionServerWrapper( server_name = 'move_base_safe_server', action_spec = MoveBaseSafeAction, wrapped_container = sm, succeeded_outcomes = ['OVERALL_SUCCESS'], aborted_outcomes = ['OVERALL_FAILED'], preempted_outcomes = ['OVERALL_PREEMPTED'], goal_key = 'move_base_safe_goal', feedback_key = 'move_base_safe_feedback', result_key = 'move_base_safe_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(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': 'SELECT_PREGRASP_PLANNER_INPUT', '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_WHOLE_BODY_MOTION', 'failure':'SET_ACTION_LIB_FAILURE', 'timeout': 'RECONFIGURE_PREGRASP_PARAMS'}) ''' smach.StateMachine.add( 'SELECT_PREGRASP_PLANNER_INPUT', send_event('/mir_manipulation/mux_pregrasp_pose/select', '/whole_body_motion_calculator_pipeline/pose_out'), transitions={'success': 'PLAN_WHOLE_BODY_MOTION'}) 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': 'PLAN_ARM_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': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'}) # based on a published pose, calls pregrasp planner to generate a graspable pose smach.StateMachine.add('PLAN_ARM_MOTION', send_event( '/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( '/pregrasp_planner_pipeline/event_out', 15.0), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', 'timeout': 'PLAN_WHOLE_BODY_MOTION', 'failure': 'PLAN_WHOLE_BODY_MOTION' }) smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION', send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'CALCULATE_BASE_MOTION'}) # 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'}) smach.StateMachine.add( 'CALCULATE_BASE_MOTION', gbs.send_and_wait_events_combined( #event_in_list=[('/base_motion_calculator/event_in','e_start'), event_in_list=[('/base_motion_calculator/event_in', 'e_start') ], #('/base_collision_checker_pipeline/base_collision_checker_node/event_in','e_start')], #event_out_list=[('/base_motion_calculator/event_out','e_success', True), event_out_list=[('/base_motion_calculator/event_out', 'e_success', True)], #('/base_collision_checker_pipeline/base_collision_checker_node/event_out','e_success', True)], timeout_duration=30), transitions={ 'success': 'STOP_PLAN_WHOLE_BODY_MOTION', 'timeout': 'STOP_MOVE_BASE_TO_OBJECT', 'failure': 'PLAN_WHOLE_BODY_MOTION' }) # select next pose from the sampled poses # 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': 'MOVE_ROBOT_TO_OBJECT'}) # execute robot motion smach.StateMachine.add( 'MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[ #('/move_arm_planned/event_in','e_start'), #('/base_motion_calculator/event_in','e_start'), ('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in', 'e_start') ], event_out_list= [ #('/move_arm_planned/event_out','e_success', True), #('/base_motion_calculator/event_out','e_success', True), ('/mcr_navigation/collision_velocity_filter/event_out', 'e_reduced_velocities_forwarded', False), ('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out', 'e_success', True) ], timeout_duration=10), transitions={ 'success': 'STOP_MOVE_BASE_TO_OBJECT', 'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure': 'STOP_MOVE_BASE_TO_OBJECT' }) # 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': 'STOP_TRAJECTORY_GENERATOR'}) # 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': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'}) smach.StateMachine.add( 'STOP_TRAJECTORY_GENERATOR', send_event('/waypoint_trajectory_generation/event_out', 'e_stop'), transitions={'success': 'SELECT_PREGRASP_PLANNER_INPUT_2'}) smach.StateMachine.add( 'SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/mir_manipulation/mux_pregrasp_pose/select', '/mcr_perception/object_selector/output/object_pose'), transitions={'success': 'PUBLISH_OBJECT_POSE'}) # pregrasp planner failed or timeout, stop the component and then return overall failure smach.StateMachine.add('PUBLISH_OBJECT_POSE', publish_object_pose(), transitions={ 'succeeded': 'PLAN_ARM_MOTION_2', 'failed': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE' }) # based on a published pose, calls pregrasp planner to generate a graspable pose smach.StateMachine.add( 'PLAN_ARM_MOTION_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_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE' }) smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event( '/pregrasp_planner_pipeline/event_in', 'e_stop'), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) # execute robot motion 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)], 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', 'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', 'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE' }) #smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'), smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', send_event('/waypoint_trajectory_generation/event_out', 'e_stop'), transitions={'success': 'SET_ACTION_LIB_FAILURE'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', send_event( '/waypoint_trajectory_generation/event_out', 'e_stop'), transitions={'success': 'CLOSE_GRIPPER'}) # close gripper 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'}) ''' # 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' }) ''' 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_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': 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS'}) smach.StateMachine.add( 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS', send_event('/mir_manipulation/mux_pregrasp_pose/select', '/mcr_perception/object_selector/output/object_pose'), transitions={'success': 'OVERALL_SUCCESS'}) # set action lib result smach.StateMachine.add( 'SET_ACTION_LIB_FAILURE', SetActionLibResult(False), transitions={'succeeded': 'MOVE_ARM_TO_HOLD_FAILURE'}) smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"), transitions={ 'succeeded': 'RESET_PREGRASP_PLANNER_INPUT_FAILURE', 'failed': 'RESET_PREGRASP_PLANNER_INPUT_FAILURE' }) smach.StateMachine.add( 'RESET_PREGRASP_PLANNER_INPUT_FAILURE', send_event('/mir_manipulation/mux_pregrasp_pose/select', '/mcr_perception/object_selector/output/object_pose'), transitions={'success': '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()