def __init__(self):
smach.StateMachine.__init__(self, outcomes=['destination_reached',
'overall_done'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'objects_goal_configuration',
'objects_to_be_grasped',
'rear_platform_occupied_poses',
'task_list'],
output_keys=['base_pose_to_approach',
'objects_goal_configuration',
'objects_to_be_grasped',
'rear_platform_occupied_poses',
'task_list'])
with self:
smach.StateMachine.add('REMOVE_WALLS_FROM_PLANNING_SCENE', gms.update_static_elements_in_planning_scene("walls", "remove"),
transitions={'succeeded':'SELECT_DELIVER_WORKSTATION'})
smach.StateMachine.add('SELECT_DELIVER_WORKSTATION', btts.select_delivery_workstation(),
transitions={'success':'MOVE_TO_DESTINATION_LOCATION',
'no_more_dest_tasks':'MOVE_TO_EXIT'})
smach.StateMachine.add('MOVE_TO_DESTINATION_LOCATION', gns.approach_pose(),
transitions={'succeeded':'ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION',
'failed':'MOVE_TO_DESTINATION_LOCATION'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION', gns.adjust_to_workspace(0.12),
transitions={'succeeded':'destination_reached',
'failed':'MOVE_TO_DESTINATION_LOCATION'})
smach.StateMachine.add('MOVE_TO_EXIT', gns.approach_pose("EXIT"),
transitions={'succeeded':'overall_done',
'failed':'MOVE_TO_EXIT'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['destination_reached',
'overall_done'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'objects_goal_configuration',
'objects_to_be_grasped',
'rear_platform_occupied_poses',
'task_list'],
output_keys=['base_pose_to_approach',
'objects_goal_configuration',
'objects_to_be_grasped',
'rear_platform_occupied_poses',
'task_list'])
with self:
smach.StateMachine.add('SELECT_DELIVER_WORKSTATION', btts.select_delivery_workstation(),
transitions={'success':'MOVE_TO_DESTINATION_LOCATION_SAFE',
'no_more_dest_tasks':'MOVE_TO_EXIT_SAFE'})
smach.StateMachine.add('MOVE_TO_DESTINATION_LOCATION_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'MOVE_TO_DESTINATION_LOCATION',
'failed': 'MOVE_TO_DESTINATION_LOCATION_SAFE'})
smach.StateMachine.add('MOVE_TO_DESTINATION_LOCATION', gns.approach_pose(),
transitions={'succeeded':'ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION',
'failed':'MOVE_TO_DESTINATION_LOCATION'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION', gns.adjust_to_workspace(0.08),
transitions={'succeeded':'destination_reached',
'failed':'MOVE_TO_DESTINATION_LOCATION'})
smach.StateMachine.add('MOVE_TO_EXIT_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'MOVE_TO_EXIT',
'failed': 'MOVE_TO_EXIT_SAFE'})
smach.StateMachine.add('MOVE_TO_EXIT', gns.approach_pose("EXIT"),
transitions={'succeeded':'overall_done',
'failed':'MOVE_TO_EXIT'})
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached',
'no_more_free_poses',
'no_more_free_poses_at_robot_platf',
'no_more_task_for_given_type'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'object_pose',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'recognized_objects',
'source_visits',
'task_list',
'vscount'],
output_keys=['base_pose_to_approach',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'source_visits',
'task_list',
'vscount'])
self.use_mockup = use_mockup
with self:
smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"),
transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION',
'no_more_task_for_given_type': 'no_more_task_for_given_type'})
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(),
transitions={'succeeded': 'PREPARE_FOR_PERCEPTION',
'failed': 'MOVE_TO_SOURCE_LOCATION'})
### start of concurrent state(s)
sm_con_prepare_for_perception = smach.Concurrence(outcomes=['succeeded', 'failed_to_adjust_base','concurrency_mapping_failure'],
default_outcome='concurrency_mapping_failure',
outcome_map={'succeeded': {'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE': 'succeeded',
'MOVE_ARM_OUT_OF_VIEW_SAFE': 'succeeded'},
'failed_to_adjust_base': {'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE': 'failed'}})
with sm_con_prepare_for_perception:
sm_sub_move_arm_safe = smach.StateMachine(outcomes=['succeeded'])
with sm_sub_move_arm_safe:
smach.StateMachine.add('ADD_WALLS_TO_PLANNING_SCENE', gms.update_static_elements_in_planning_scene("walls", "add"),
transitions={'succeeded':'MOVE_ARM_OUT_OF_VIEW'})
smach.StateMachine.add('MOVE_ARM_OUT_OF_VIEW', gms.move_arm('out_of_view'),
transitions={'succeeded':'succeeded',
'failed':'MOVE_ARM_OUT_OF_VIEW'})
smach.Concurrence.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.15))
smach.Concurrence.add('MOVE_ARM_OUT_OF_VIEW_SAFE', sm_sub_move_arm_safe)
smach.StateMachine.add('PREPARE_FOR_PERCEPTION', sm_con_prepare_for_perception, transitions={'succeeded': 'RECOGNIZE_OBJECTS',
'failed_to_adjust_base': 'MOVE_TO_SOURCE_LOCATION',
'concurrency_mapping_failure': 'PREPARE_FOR_PERCEPTION'})
### end of concurrent state(s)
smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=3, frame_id='/odom'),
transitions={'objects_found':'SELECT_OBJECT_TO_BE_GRASPED',
'no_objects_found':'SHIFT_BASE_RANDOM'},
remapping={'found_objects':'recognized_objects'})
smach.StateMachine.add('SHIFT_BASE_RANDOM', gns.move_base_relative([0.0, 0.0, -0.03, 0.03, 0.0, 0.0]),
transitions={'succeeded': 'RECOGNIZE_OBJECTS_LOOP',
'timeout': 'RECOGNIZE_OBJECTS_LOOP'})
#FIXME: Is there a loop reset?
smach.StateMachine.add('RECOGNIZE_OBJECTS_LOOP', gbs.loop_for(1),
transitions={'loop': 'RECOGNIZE_OBJECTS',
#'continue': 'SHIFT_BASE_RANDOM'}) # For BMT
'continue': 'SKIP_SOURCE_POSE'}) # For BTT
smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(),
transitions={'obj_selected':'PREPARE_FOR_GRASPING',
'no_obj_selected':'SKIP_SOURCE_POSE',
'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'})
### start of concurrent state(s)
sm_con_prepare_for_grasping = smach.Concurrence(outcomes=['succeeded', 'tf_error_in_computing_base_shift','concurrency_mapping_failure'],
default_outcome='concurrency_mapping_failure',
outcome_map={'succeeded': {'ALIGN_BASE_WITH_OBJECT': 'succeeded',
'ADD_WALLS_TO_PLANNING_SCENE': 'succeeded'},
'tf_error_in_computing_base_shift': {'ALIGN_BASE_WITH_OBJECT': 'tf_error_in_computing_base_shift'}},
input_keys=['object_to_grasp','move_base_by'],
output_keys=['move_base_by'])
with sm_con_prepare_for_grasping:
sm_sub_shift_base = smach.StateMachine(outcomes=['succeeded', 'tf_error_in_computing_base_shift'],
input_keys=['object_to_grasp','move_base_by'],
output_keys=['move_base_by'])
with sm_sub_shift_base:
smach.StateMachine.add('COMPUTE_BASE_SHIFT_TO_OBJECT', btts.compute_base_shift_to_object(),
transitions={'succeeded': 'MOVE_BASE_RELATIVE',
'tf_error': 'tf_error_in_computing_base_shift'},
remapping={'object_pose': 'object_to_grasp'})
smach.StateMachine.add('MOVE_BASE_RELATIVE', gns.move_base_relative(),
transitions={'succeeded': 'succeeded',
'timeout': 'MOVE_BASE_RELATIVE'})
smach.Concurrence.add('ALIGN_BASE_WITH_OBJECT', sm_sub_shift_base)
smach.Concurrence.add('ADD_WALLS_TO_PLANNING_SCENE', gms.update_static_elements_in_planning_scene("walls", "add"))
smach.StateMachine.add('PREPARE_FOR_GRASPING', sm_con_prepare_for_grasping,
transitions={'succeeded': 'MOVE_ARM_TO_PREGRASP',
'tf_error_in_computing_base_shift': 'PREPARE_FOR_PERCEPTION',
'concurrency_mapping_failure': 'PREPARE_FOR_GRASPING'})
### end of concurrent state(s)
smach.StateMachine.add('MOVE_ARM_TO_PREGRASP', gms.move_arm("pre_grasp"),
transitions={'succeeded': 'DO_VISUAL_SERVERING',
'failed': 'MOVE_ARM_TO_PREGRASP'})
# state skipped
smach.StateMachine.add('DO_VISUAL_SERVERING', gps.do_visual_servoing(),
transitions={'succeeded':'GRASP_OBJ',
'failed':'VISUAL_SERVOING_LOOP',
'timeout':'VISUAL_SERVOING_LOOP',
'lost_object': 'VISUAL_SERVOING_LOOP'})
smach.StateMachine.add('VISUAL_SERVOING_LOOP', btts.loop_for(max_loop_count=1),
transitions={'loop': 'HELP_VISUAL_SERVOING',
'continue': 'SKIP_SOURCE_POSE'})
smach.StateMachine.add('HELP_VISUAL_SERVOING', gns.adjust_to_workspace(0.12),
transitions={'succeeded':'MOVE_ARM_TO_PREGRASP',
'failed':'MOVE_ARM_TO_PREGRASP'})
if (self.use_mockup):
smach.StateMachine.add('GRASP_OBJ', gms.grasp_object(),
transitions={'succeeded':'ATTACH_OBJECT_TO_ROBOT',
'failed':'SKIP_SOURCE_POSE'})
smach.StateMachine.add('ATTACH_OBJECT_TO_ROBOT', gms.update_robot_planning_scene("attach"),
transitions={'succeeded':'REMOVE_OBJECT_FROM_MOCKUP'},
remapping={'object': 'object_to_grasp'})
smach.StateMachine.add("REMOVE_OBJECT_FROM_MOCKUP",
perception_mockup_util.remove_object_to_grasp_state(),
transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM'})
else:
smach.StateMachine.add('GRASP_OBJ', gms.grasp_object(),
transitions={'succeeded':'ATTACH_OBJECT_TO_ROBOT',
'failed':'SKIP_SOURCE_POSE'})
smach.StateMachine.add('ATTACH_OBJECT_TO_ROBOT', gms.update_robot_planning_scene("attach"),
transitions={'succeeded':'PLACE_OBJ_ON_REAR_PLATFORM'},
remapping={'object': 'object_to_grasp'})
smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(),
transitions={'succeeded':'DETACH_OBJECT_FROM_ROBOT',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('DETACH_OBJECT_FROM_ROBOT', gms.update_robot_planning_scene("load"),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'},
remapping={'object': 'object_to_grasp'})
# MISC STATES
smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'),
transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK',
'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached',
'no_more_free_poses',
'no_more_free_poses_at_robot_platf',
'no_more_task_for_given_type'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_pose',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'recognized_objects',
'source_visits',
'task_list',
'test',
'vscount'],
output_keys=['base_pose_to_approach',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'source_visits',
'task_list',
'test',
'vscount'])
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"),
transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION_SAFE',
'no_more_task_for_given_type': 'no_more_task_for_given_type'})
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'MOVE_TO_SOURCE_LOCATION',
'failed': 'MOVE_TO_SOURCE_LOCATION_SAFE'})
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(),
transitions={'succeeded': 'STOP_ALL_COMPONENTS_AT_START',
'failed': 'MOVE_TO_SOURCE_LOCATION'})
smach.StateMachine.add('STOP_ALL_COMPONENTS_AT_START', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.2),
transitions={'succeeded':'SELECT_NEXT_LOOK_POSE',
'failed':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace_right', 'look_at_workspace','look_at_workspace_left']),
transitions={'succeeded': 'LOOK_AROUND',
'failed': 'RECOGNIZE_OBJECTS'})
smach.StateMachine.add('LOOK_AROUND', gms.move_arm(),
transitions={'succeeded': 'RECOGNIZE_OBJECTS',
'failed': 'LOOK_AROUND'})
smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1),
transitions={'objects_found': 'ACCUMULATE_RECOGNIZED_LISTS',
'no_objects_found':'ACCUMULATE_RECOGNIZED_LISTS'},
remapping={'found_objects':'recognized_objects'})
smach.StateMachine.add('ACCUMULATE_RECOGNIZED_LISTS', gps.accumulate_recognized_objects_list(),
transitions={'complete': 'SELECT_OBJECT_TO_BE_GRASPED',
'merged':'SELECT_NEXT_LOOK_POSE'})
smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(),
transitions={'obj_selected':'COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'no_obj_selected':'SKIP_SOURCE_POSE',
'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'})
smach.StateMachine.add('COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/pregrasp_planner/event_in','e_start'),
('/gripper_to_object_pose_error_calculator/event_in','e_start'),
('/mcr_common/relative_displacement_calculator/event_in','e_start')]),
transitions={'success':'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT'})
smach.StateMachine.add('WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.wait_for_events([('/pregrasp_planner/event_out','e_success', True),
('/gripper_to_object_pose_error_calculator/event_out','e_success', True),
('/mcr_common/relative_displacement_calculator/event_out','e_done', True)]),
transitions={'success':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'timeout': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'MOVE_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('MOVE_ARM_AND_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_start'),
('/planned_motion/event_in', 'e_start')]),
transitions={'success':'WAIT_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('WAIT_ARM_AND_BASE_TO_OBJECT', gbs.wait_for_events([('/mcr_navigation/relative_base_controller/event_out','e_done', True),
('/mcr_navigation/collision_velocity_filter/event_out','e_zero_velocities_forwarded', False),
('/planned_motion/event_out', 'e_success', True)]),
transitions={'success':'STOP_MOVE_ARM_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={'success':'GRASP_OBJ'}) # comment out if using visual servoing
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('ALIGN_WITH_OBJECT_LOOP', mir_gbs.loop_for_vs(1),
transitions={'loop': 'START_VISUAL_SERVOING',
'continue': 'GRASP_OBJ'})
smach.StateMachine.add('START_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_start')]),
transitions={'success':'WAIT_VISUAL_SERVOING'})
smach.StateMachine.add('WAIT_VISUAL_SERVOING', gbs.wait_for_events([('/mcr_monitoring/component_wise_pose_error_monitor/event_out','e_stop', True)], timeout_duration=30),
transitions={'success':'STOP_VISUAL_SERVOING',
'timeout': 'STOP_VISUAL_SERVOING_WITH_FAILURE',
'failure':'STOP_VISUAL_SERVOING_WITH_FAILURE'})
smach.StateMachine.add('STOP_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_SUCCESS'})
smach.StateMachine.add('STOP_VISUAL_SERVOING_WITH_FAILURE', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_FAILURE'})
smach.StateMachine.add('SET_VISUAL_SERVOING_SUCCESS', mir_gbs.set_vs_status(status=True),
transitions={'success':'OPEN_GRIPPER_FOR_APPROACH_OBJECT'})
smach.StateMachine.add('SET_VISUAL_SERVOING_FAILURE', mir_gbs.set_vs_status(status=False),
transitions={'success':'SELECT_OBJECT_TO_BE_GRASPED'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_APPROACH_OBJECT', gms.control_gripper('open'),
transitions={'succeeded':'APPROACH_OBJECT'})
smach.StateMachine.add('APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_start')]),
transitions={'success':'WAIT_APPROACH_OBJECT'})
smach.StateMachine.add('WAIT_APPROACH_OBJECT', gbs.wait_for_events([('/guarded_approach_pose/coordinator/event_out','e_success', True)], timeout_duration=6),
transitions={'success':'STOP_APPROACH_OBJECT',
'timeout': 'STOP_APPROACH_OBJECT',
'failure':'STOP_APPROACH_OBJECT'})
smach.StateMachine.add('STOP_APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_stop')]),
transitions={'success':'CLOSE_GRIPPER'})
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM'})
smach.StateMachine.add('GRASP_OBJ', gms.linear_motion(operation='grasp'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('GRASP_OBJ_VISUAL_SERVOING', gms.linear_motion(operation='grasp', offset_x=-0.05),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('PRE_PLACE_OBJ_ON_REAR_PLATFORM', btts.pre_place_obj_on_rear_platform_btt(),
transitions={'succeeded':'VERIFY_GRASPED',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('VERIFY_GRASPED', gbs.send_event([('/gripper_controller/grasp_monitor/event_in', 'e_trigger')]),
transitions={'success':'WAIT_FOR_VERIFY_GRASPED'})
smach.StateMachine.add('WAIT_FOR_VERIFY_GRASPED', gbs.wait_for_events([('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5),
transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM',
'timeout':'OPEN_GRIPPER_FOR_GRASP_FAILURE',
'failure':'OPEN_GRIPPER_FOR_GRASP_FAILURE'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_GRASP_FAILURE', gms.control_gripper('open'),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS'})
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('DELETE_FROM_RECOGNIZED_OBJECTS', btts.delete_from_recognized_objects(),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'},
remapping={'object_to_delete': 'object_to_grasp'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_NEXT', gns.adjust_to_workspace(0.1),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED',
'failed':'ADJUST_POSE_WRT_WORKSPACE_NEXT'})
smach.StateMachine.add('NO_MORE_FREE_POSES_SAFE',gms.move_arm('look_at_workspace'),
transitions={'succeeded':'no_more_free_poses',
'failed':'SKIP_SOURCE_POSE_SAFE'})
# MISC STATES
smach.StateMachine.add('SKIP_SOURCE_POSE_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded':'SKIP_SOURCE_POSE',
'failed':'SKIP_SOURCE_POSE_SAFE'})
smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'),
transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK',
'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["destination_reached", "overall_done"],
input_keys=[
"base_pose_to_approach",
"desired_distance_to_workspace",
"objects_goal_configuration",
"objects_to_be_grasped",
"rear_platform_occupied_poses",
"task_list",
],
output_keys=[
"base_pose_to_approach",
"objects_goal_configuration",
"objects_to_be_grasped",
"rear_platform_occupied_poses",
"task_list",
],
)
with self:
smach.StateMachine.add(
"SELECT_DELIVER_WORKSTATION",
btts.select_delivery_workstation(),
transitions={
"success": "MOVE_TO_DESTINATION_LOCATION_SAFE",
"no_more_dest_tasks": "MOVE_TO_EXIT_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_DESTINATION_LOCATION_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "MOVE_TO_DESTINATION_LOCATION",
"failed": "MOVE_TO_DESTINATION_LOCATION_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_DESTINATION_LOCATION",
gns.approach_pose(),
transitions={
"succeeded": "ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION",
"failed": "MOVE_TO_DESTINATION_LOCATION",
},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_AT_DESTINATION",
gns.adjust_to_workspace(0.08),
transitions={
"succeeded": "destination_reached",
"failed": "MOVE_TO_DESTINATION_LOCATION",
},
)
smach.StateMachine.add(
"MOVE_TO_EXIT_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "MOVE_TO_EXIT",
"failed": "MOVE_TO_EXIT_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_EXIT",
gns.approach_pose("EXIT"),
transitions={
"succeeded": "overall_done",
"failed": "MOVE_TO_EXIT",
},
)
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(
self,
outcomes=[
"pose_skipped_but_platform_limit_reached",
"no_more_free_poses",
"no_more_free_poses_at_robot_platf",
"no_more_task_for_given_type",
],
input_keys=[
"base_pose_to_approach",
"desired_distance_to_workspace",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_pose",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"recognized_objects",
"source_visits",
"task_list",
"test",
"vscount",
],
output_keys=[
"base_pose_to_approach",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"source_visits",
"task_list",
"test",
"vscount",
],
)
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add(
"SELECT_SOURCE_SUBTASK",
btts.select_btt_subtask(type="source"),
transitions={
"task_selected": "MOVE_TO_SOURCE_LOCATION_SAFE",
"no_more_task_for_given_type":
"no_more_task_for_given_type",
},
)
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "MOVE_TO_SOURCE_LOCATION",
"failed": "MOVE_TO_SOURCE_LOCATION_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION",
gns.approach_pose(),
transitions={
"succeeded": "STOP_ALL_COMPONENTS_AT_START",
"failed": "MOVE_TO_SOURCE_LOCATION",
},
)
smach.StateMachine.add(
"STOP_ALL_COMPONENTS_AT_START",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
gns.adjust_to_workspace(0.2),
transitions={
"succeeded": "SELECT_NEXT_LOOK_POSE",
"failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
},
)
smach.StateMachine.add(
"SELECT_NEXT_LOOK_POSE",
gms.select_arm_pose([
"look_at_workspace_right",
"look_at_workspace",
"look_at_workspace_left",
]),
transitions={
"succeeded": "LOOK_AROUND",
"failed": "RECOGNIZE_OBJECTS",
},
)
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm(),
transitions={
"succeeded": "RECOGNIZE_OBJECTS",
"failed": "LOOK_AROUND",
},
)
smach.StateMachine.add(
"RECOGNIZE_OBJECTS",
gps.find_objects(retries=1),
transitions={
"objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
"no_objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
},
remapping={"found_objects": "recognized_objects"},
)
smach.StateMachine.add(
"ACCUMULATE_RECOGNIZED_LISTS",
gps.accumulate_recognized_objects_list(),
transitions={
"complete": "SELECT_OBJECT_TO_BE_GRASPED",
"merged": "SELECT_NEXT_LOOK_POSE",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_TO_BE_GRASPED",
btts.select_object_to_be_grasped(),
transitions={
"obj_selected":
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"no_obj_selected":
"SKIP_SOURCE_POSE",
"no_more_free_poses_at_robot_platf":
"no_more_free_poses_at_robot_platf",
},
)
smach.StateMachine.add(
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
("/pregrasp_planner/event_in", "e_start"),
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_start",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_start",
),
]),
transitions={
"success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT"
},
)
smach.StateMachine.add(
"WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.wait_for_events([
("/pregrasp_planner/event_out", "e_success", True),
(
"/gripper_to_object_pose_error_calculator/event_out",
"e_success",
True,
),
(
"/mcr_common/relative_displacement_calculator/event_out",
"e_done",
True,
),
]),
transitions={
"success":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"timeout":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
"failure":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "MOVE_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"MOVE_ARM_AND_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_start",
),
("/planned_motion/event_in", "e_start"),
]),
transitions={"success": "WAIT_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"WAIT_ARM_AND_BASE_TO_OBJECT",
gbs.wait_for_events([
(
"/mcr_navigation/relative_base_controller/event_out",
"e_done",
True,
),
(
"/mcr_navigation/collision_velocity_filter/event_out",
"e_zero_velocities_forwarded",
False,
),
("/planned_motion/event_out", "e_success", True),
]),
transitions={
"success": "STOP_MOVE_ARM_BASE_TO_OBJECT",
"timeout": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={"success": "GRASP_OBJ"},
) # comment out if using visual servoing
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"ALIGN_WITH_OBJECT_LOOP",
mir_gbs.loop_for_vs(1),
transitions={
"loop": "START_VISUAL_SERVOING",
"continue": "GRASP_OBJ",
},
)
smach.StateMachine.add(
"START_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_start")]),
transitions={"success": "WAIT_VISUAL_SERVOING"},
)
smach.StateMachine.add(
"WAIT_VISUAL_SERVOING",
gbs.wait_for_events(
[(
"/mcr_monitoring/component_wise_pose_error_monitor/event_out",
"e_stop",
True,
)],
timeout_duration=30,
),
transitions={
"success": "STOP_VISUAL_SERVOING",
"timeout": "STOP_VISUAL_SERVOING_WITH_FAILURE",
"failure": "STOP_VISUAL_SERVOING_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_SUCCESS"},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING_WITH_FAILURE",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_FAILURE"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_SUCCESS",
mir_gbs.set_vs_status(status=True),
transitions={"success": "OPEN_GRIPPER_FOR_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_FAILURE",
mir_gbs.set_vs_status(status=False),
transitions={"success": "SELECT_OBJECT_TO_BE_GRASPED"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_APPROACH_OBJECT",
gms.control_gripper("open"),
transitions={"succeeded": "APPROACH_OBJECT"},
)
smach.StateMachine.add(
"APPROACH_OBJECT",
gbs.send_event([(
"/guarded_approach_pose/coordinator/event_in",
"e_start",
)]),
transitions={"success": "WAIT_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"WAIT_APPROACH_OBJECT",
gbs.wait_for_events(
[(
"/guarded_approach_pose/coordinator/event_out",
"e_success",
True,
)],
timeout_duration=6,
),
transitions={
"success": "STOP_APPROACH_OBJECT",
"timeout": "STOP_APPROACH_OBJECT",
"failure": "STOP_APPROACH_OBJECT",
},
)
smach.StateMachine.add(
"STOP_APPROACH_OBJECT",
gbs.send_event([("/guarded_approach_pose/coordinator/event_in",
"e_stop")]),
transitions={"success": "CLOSE_GRIPPER"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM"},
)
smach.StateMachine.add(
"GRASP_OBJ",
gms.linear_motion(operation="grasp"),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"GRASP_OBJ_VISUAL_SERVOING",
gms.linear_motion(operation="grasp", offset_x=-0.05),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"PRE_PLACE_OBJ_ON_REAR_PLATFORM",
btts.pre_place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "VERIFY_GRASPED",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"VERIFY_GRASPED",
gbs.send_event([(
"/gripper_controller/grasp_monitor/event_in",
"e_trigger",
)]),
transitions={"success": "WAIT_FOR_VERIFY_GRASPED"},
)
smach.StateMachine.add(
"WAIT_FOR_VERIFY_GRASPED",
gbs.wait_for_events(
[(
"/gripper_controller/grasp_monitor/event_out",
"e_object_grasped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "PLACE_OBJ_ON_REAR_PLATFORM",
"timeout": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
"failure": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_GRASP_FAILURE",
gms.control_gripper("open"),
transitions={"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add(
"PLACE_OBJ_ON_REAR_PLATFORM",
btts.place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"DELETE_FROM_RECOGNIZED_OBJECTS",
btts.delete_from_recognized_objects(),
transitions={"succeeded": "SELECT_OBJECT_TO_BE_GRASPED"},
remapping={"object_to_delete": "object_to_grasp"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_NEXT",
gns.adjust_to_workspace(0.1),
transitions={
"succeeded": "SELECT_OBJECT_TO_BE_GRASPED",
"failed": "ADJUST_POSE_WRT_WORKSPACE_NEXT",
},
)
smach.StateMachine.add(
"NO_MORE_FREE_POSES_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "no_more_free_poses",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
# MISC STATES
smach.StateMachine.add(
"SKIP_SOURCE_POSE_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "SKIP_SOURCE_POSE",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
smach.StateMachine.add(
"SKIP_SOURCE_POSE",
btts.skip_pose("source"),
transitions={
"pose_skipped":
"SELECT_SOURCE_SUBTASK",
"pose_skipped_but_platform_limit_reached":
"pose_skipped_but_platform_limit_reached",
},
)