def main():
rospy.init_node('compute_base_shift_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['compute_base_shift_goal'],
output_keys=[
'compute_base_shift_feedback',
'compute_base_shift_result'
])
sm.userdata.next_arm_pose_index = 0
sm.userdata.move_arm_to = None
with sm:
#add states to the container
# remove accumulated base poses and start non loop dependant components
smach.StateMachine.add(
'CLEAR_BASE_POSES',
gbs.send_event([
('/move_base_relative/pose_selector/event_in', 'e_forget'),
('/move_base_relative/random_obj_selector_transformer_republisher/event_in',
'e_start'),
('/move_base_relative/modified_pose_transformer/event_in',
'e_start')
]),
transitions={'success': 'SELECT_NEXT_LOOK_POSE'})
# select a look_at_workspace pose
smach.StateMachine.add(
'SELECT_NEXT_LOOK_POSE',
gms.select_arm_pose(['look_at_workspace']),
transitions={
'succeeded': 'LOOK_AROUND', # next pose selected
'completed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS',
'failed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS'
}
) # we've run out of poses to select, which means we've gone through the list
# move arm to selected pose
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm(),
transitions={
'succeeded': 'RECOGNIZE_OBJECTS',
'failed': 'LOOK_AROUND'
})
smach.StateMachine.add(
'RECOGNIZE_OBJECTS',
gps.find_objects(retries=1),
transitions={
'objects_found': 'SELECT_OBJECT',
'no_objects_found': 'SELECT_NEXT_LOOK_POSE'
},
remapping={'found_objects': 'recognized_objects'})
# here the object selector is set up to select objects randomly
# it will publish object name (goes to pose selector) and object pose (goes to compute base shift pipeline)
# relative displacement calculator pose republisher in odom frame is also triggered here, before doing computations
smach.StateMachine.add(
'SELECT_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/random_object_selector/event_in',
'e_trigger')
],
event_out_list=[
('/mcr_perception/random_object_selector/event_out',
'e_selected', True)
],
timeout_duration=10),
transitions={
'success': 'SELECT_OBJECT',
'timeout': 'SELECT_NEXT_LOOK_POSE',
'failure': 'SELECT_NEXT_LOOK_POSE'
}
) # this means we're done computing poses for all recognized objects from this pose
# stop relative displacement calculator pose republisher and after that go to success
smach.StateMachine.add(
'STOP_REMAINING_COMPONENTS_WITH_SUCCESS',
gbs.send_event([
('/move_base_relative/random_obj_selector_transformer_republisher/event_in',
'e_stop'),
('/move_base_relative/modified_pose_transformer/event_in',
'e_stop')
]),
transitions={'success': 'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# Construct action server wrapper
asw = ActionServerWrapper(server_name='compute_base_shift_server',
action_spec=ComputeBaseShiftAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='compute_base_shift_goal',
feedback_key='compute_base_shift_feedback',
result_key='compute_base_shift_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('perceive_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_cavity_goal'],
output_keys=['perceive_cavity_feedback', 'perceive_cavity_result'])
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME_FOR_WBC'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME_FOR_WBC',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'START_POSE_SELECTOR'})
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
'START_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/cavity_pose_selector/event_in',
'e_start')]),
transitions={'success': 'SELECT_NEXT_LOOK_POSE'})
# select a look_at_workspace pose
smach.StateMachine.add(
'SELECT_NEXT_LOOK_POSE',
gms.select_arm_pose([
'look_straight_at_workspace_left',
'look_straight_at_workspace_right'
]),
transitions={
'succeeded': 'LOOK_AROUND', # next pose selected
'completed':
'SET_ACTION_LIB_SUCCESS', # we've run out of poses to select, which means we've gone through the list
'failed': 'SET_ACTION_LIB_SUCCESS'
}
) # we've run out of poses to select, which means we've gone through the list
# move arm to selected pose
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm(),
transitions={
'succeeded': 'RECOGNIZE_CAVITIES',
'failed': 'LOOK_AROUND'
})
# trigger perception pipeline
smach.StateMachine.add('RECOGNIZE_CAVITIES',
gps.find_cavities(retries=1),
transitions={
'cavities_found': 'SELECT_NEXT_LOOK_POSE',
'no_cavities_found': 'SELECT_NEXT_LOOK_POSE'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('perceive_cavity_smach_viewer', sm,
'/PERCEIVE_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_cavity_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_cavity_goal',
feedback_key='perceive_cavity_feedback',
result_key='perceive_cavity_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'failed', 'no_object_for_ppt_platform'],
input_keys=[
'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to',
'move_base_by', 'object_pose', 'rear_platform_free_poses',
'rear_platform_occupied_poses', 'selected_objects',
'next_arm_pose_index', 'cavity_pose', 'task_list'
],
output_keys=[
'base_pose_to_approach', 'last_grasped_obj', 'move_arm_to',
'move_base_by', 'rear_platform_free_poses',
'rear_platform_occupied_poses', 'selected_objects',
'next_arm_pose_index', 'cavity_pose', 'task_list'
])
with self:
# TODO: ENSURE WE HAVE ENOUGH SPACE IN THE ARENA
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE',
gns.adjust_to_workspace(0.2),
transitions={
'succeeded':
'SELECT_OBJECTS_TO_PLACE',
'failed':
'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'
})
# Select object from task list which needs to be place in cavities.
smach.StateMachine.add('SELECT_OBJECTS_TO_PLACE',
ppts.select_objects_to_place(),
transitions={
'objects_selected':
'CLEAR_CAVITIES',
'no_more_obj_for_this_workspace':
'no_object_for_ppt_platform'
})
smach.StateMachine.add(
'CLEAR_CAVITIES',
ppts.clear_cavities(),
transitions={'succeeded': 'SELECT_NEXT_LOOK_POSE'})
# CAVITY RECOGNITION PIPELINE
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': 'CHECK_FOUND_CAVITIES'
})
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm(),
transitions={
'succeeded': 'FIND_CAVITIES_LOOP',
'failed': 'LOOK_AROUND'
})
smach.StateMachine.add('FIND_CAVITIES_LOOP',
gbs.loop_for(3),
transitions={
'loop': 'FIND_CAVITIES',
'continue': 'CHECK_FOUND_CAVITIES'
})
smach.StateMachine.add('FIND_CAVITIES',
gps.find_cavities(),
transitions={
'succeeded': 'TRANSFORM_CAVITIES',
'failed': 'SELECT_NEXT_LOOK_POSE'
})
smach.StateMachine.add(
'TRANSFORM_CAVITIES',
gps.transform_object_poses(frame_id='/odom'),
transitions={'succeeded': 'FIND_BEST_MATCHED_CAVITIES'},
remapping={'found_objects': 'found_cavities'})
smach.StateMachine.add('FIND_BEST_MATCHED_CAVITIES',
gps.find_best_matched_cavities(),
transitions={
'succeeded': 'SELECT_NEXT_LOOK_POSE',
'complete': 'CHECK_FOUND_CAVITIES'
})
smach.StateMachine.add('CHECK_FOUND_CAVITIES',
gps.check_found_cavities(),
transitions={
'cavities_found':
'STOP_ALL_COMPONENTS_AT_START',
'no_cavities_found': 'failed'
})
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'),
('/pregrasp_arm_monitor/event_in', 'e_stop'),
('/mcr_navigation/relative_base_controller/event_in',
'e_stop'), ('/planned_motion/event_in', 'e_stop')
]),
transitions={'success': 'SELECT_OBJECT_TO_PLACE'})
# OBJECT PLACING pipeline
# select cavitiy based on recognized cavities and list of objects to place in cavity.
smach.StateMachine.add(
'SELECT_OBJECT_TO_PLACE',
ppts.select_object_to_place(),
transitions={
'object_selected': 'COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
'no_more_objects': 'succeeded',
'no_more_cavities': 'succeeded'
},
remapping={'found_objects': 'best_matched_cavities'})
# ARM AND BASE ALIGNMENT TO CAVITY PIPELINE
smach.StateMachine.add(
'COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
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_CAVITY'
})
smach.StateMachine.add(
'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
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_CAVITY',
'timeout':
'STOP_ALL_COMPONENTS', #should we try again for compute arm base shift
'failure': 'STOP_ALL_COMPONENTS'
}) #should we try again for compute arm base shift
smach.StateMachine.add(
'STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY',
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': 'ALIGN_BASE_WITH_CAVITY'})
smach.StateMachine.add(
'ALIGN_BASE_WITH_CAVITY',
gbs.send_event([
('/mcr_navigation/relative_base_controller/event_in',
'e_start')
]),
transitions={'success': 'WAIT_ALIGN_BASE_WITH_CAVITY'})
smach.StateMachine.add(
'WAIT_ALIGN_BASE_WITH_CAVITY',
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)
]),
transitions={
'success': 'STOP_ALIGN_BASE_WITH_CAVITY',
'timeout':
'STOP_ALL_COMPONENTS', #should we try again for compute and aligh arm and base to object
'failure': 'STOP_ALL_COMPONENTS'
}
) #should we try again for compute and aligh arm and base to object
smach.StateMachine.add(
'STOP_ALIGN_BASE_WITH_CAVITY',
gbs.send_event([
('/mcr_navigation/relative_base_controller/event_in',
'e_stop')
]),
transitions={'success': 'GRASP_OBJECT_FOR_CAVITY_FROM_PLTF'})
# make sure that gripper is open
smach.StateMachine.add('GRASP_OBJECT_FOR_CAVITY_FROM_PLTF',
ppts.grasp_obj_for_hole_from_pltf(),
transitions={
'object_grasped':
'MOVE_ARM_TO_CAVITY',
'no_more_obj_for_this_workspace':
'no_object_for_ppt_platform'
})
smach.StateMachine.add('GO_TO_PREGRASP',
gms.move_arm('pre_grasp'),
transitions={
'succeeded': 'MOVE_ARM_TO_CAVITY',
'failed': 'MOVE_ARM_TO_CAVITY'
})
smach.StateMachine.add(
'MOVE_ARM_TO_CAVITY',
gbs.send_event([
('/planned_motion/event_in', 'e_start'),
#smach.StateMachine.add('MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in', 'e_start')]),
('/pregrasp_arm_monitor/event_in', 'e_start')
]),
transitions={'success': 'WAIT_MOVE_ARM_TO_CAVITY'})
smach.StateMachine.add(
'WAIT_MOVE_ARM_TO_CAVITY',
gbs.wait_for_events([('/pregrasp_arm_monitor/event_out',
'e_done', True)]),
# smach.StateMachine.add('WAIT_MOVE_ARM_TO_CAVITY', gbs.wait_for_events([('/planned_motion/event_out','e_success', True)]),
# ('/pregrasp_arm_monitor/event_out', 'e_done', True)]),
transitions={
'success': 'STOP_MOVE_ARM_TO_CAVITY',
'timeout':
'STOP_ALL_COMPONENTS', #should we try again for compute and aligh arm and base to object
'failure': 'STOP_ALL_COMPONENTS'
}
) #should we try again for compute and aligh arm and base to object
#smach.StateMachine.add('STOP_MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in','e_stop')]),
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_CAVITY',
gbs.send_event([('/planned_motion/event_in', 'e_stop'),
('/pregrasp_arm_monitor/event_in', 'e_stop')]),
transitions={'success': 'MOVE_ARM_TO_INTERMEDIATE_POSE'})
smach.StateMachine.add('PLACE_OBJECT',
gms.linear_motion(operation='release'),
transitions={
'succeeded': 'WIGGLE_ARM_LEFT',
'failed': 'PLACE_OBJECT'
})
smach.StateMachine.add('WIGGLE_ARM_LEFT',
ppts.ppt_wiggle_arm(wiggle_offset=-0.12),
transitions={
'succeeded': 'WIGGLE_ARM_RIGHT',
'failed': 'WIGGLE_ARM_RIGHT'
})
smach.StateMachine.add('WIGGLE_ARM_RIGHT',
ppts.ppt_wiggle_arm(wiggle_offset=0.24),
transitions={
'succeeded':
'MOVE_ARM_TO_INTERMEDIATE_POSE',
'failed':
'MOVE_ARM_TO_INTERMEDIATE_POSE'
})
smach.StateMachine.add('MOVE_ARM_TO_INTERMEDIATE_POSE',
gms.move_arm('look_at_workspace'),
transitions={
'succeeded': 'SELECT_OBJECT_TO_PLACE',
'failed':
'MOVE_ARM_TO_INTERMEDIATE_POSE'
})
#normal placing if arm and base alignment pipeline fails
smach.StateMachine.add(
'STOP_ALL_COMPONENTS',
gbs.send_event([
('/gripper_to_object_pose_error_calculator/event_in',
'e_stop'), ('/pregrasp_arm_monitor/event_in', 'e_stop'),
('/mcr_common/relative_displacement_calculator/event_in',
'e_stop'),
('/mcr_navigation/relative_base_controller/event_in',
'e_stop'), ('/planned_motion/event_in', 'e_stop')
]),
transitions={'success': 'SELECT_OBJECT_TO_PLACE'})
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached',
'no_more_free_poses',
'no_more_free_poses_at_robot_platf',
'no_more_task_for_given_type'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_pose',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'recognized_objects',
'source_visits',
'task_list',
'test',
'vscount'],
output_keys=['base_pose_to_approach',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'source_visits',
'task_list',
'test',
'vscount'])
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"),
transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION_SAFE',
'no_more_task_for_given_type': 'no_more_task_for_given_type'})
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'MOVE_TO_SOURCE_LOCATION',
'failed': 'MOVE_TO_SOURCE_LOCATION_SAFE'})
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(),
transitions={'succeeded': 'STOP_ALL_COMPONENTS_AT_START',
'failed': 'MOVE_TO_SOURCE_LOCATION'})
smach.StateMachine.add('STOP_ALL_COMPONENTS_AT_START', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.2),
transitions={'succeeded':'SELECT_NEXT_LOOK_POSE',
'failed':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace_right', 'look_at_workspace','look_at_workspace_left']),
transitions={'succeeded': 'LOOK_AROUND',
'failed': 'RECOGNIZE_OBJECTS'})
smach.StateMachine.add('LOOK_AROUND', gms.move_arm(),
transitions={'succeeded': 'RECOGNIZE_OBJECTS',
'failed': 'LOOK_AROUND'})
smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1),
transitions={'objects_found': 'ACCUMULATE_RECOGNIZED_LISTS',
'no_objects_found':'ACCUMULATE_RECOGNIZED_LISTS'},
remapping={'found_objects':'recognized_objects'})
smach.StateMachine.add('ACCUMULATE_RECOGNIZED_LISTS', gps.accumulate_recognized_objects_list(),
transitions={'complete': 'SELECT_OBJECT_TO_BE_GRASPED',
'merged':'SELECT_NEXT_LOOK_POSE'})
smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(),
transitions={'obj_selected':'COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'no_obj_selected':'SKIP_SOURCE_POSE',
'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'})
smach.StateMachine.add('COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/pregrasp_planner/event_in','e_start'),
('/gripper_to_object_pose_error_calculator/event_in','e_start'),
('/mcr_common/relative_displacement_calculator/event_in','e_start')]),
transitions={'success':'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT'})
smach.StateMachine.add('WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.wait_for_events([('/pregrasp_planner/event_out','e_success', True),
('/gripper_to_object_pose_error_calculator/event_out','e_success', True),
('/mcr_common/relative_displacement_calculator/event_out','e_done', True)]),
transitions={'success':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'timeout': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'MOVE_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('MOVE_ARM_AND_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_start'),
('/planned_motion/event_in', 'e_start')]),
transitions={'success':'WAIT_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('WAIT_ARM_AND_BASE_TO_OBJECT', gbs.wait_for_events([('/mcr_navigation/relative_base_controller/event_out','e_done', True),
('/mcr_navigation/collision_velocity_filter/event_out','e_zero_velocities_forwarded', False),
('/planned_motion/event_out', 'e_success', True)]),
transitions={'success':'STOP_MOVE_ARM_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={'success':'GRASP_OBJ'}) # comment out if using visual servoing
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('ALIGN_WITH_OBJECT_LOOP', mir_gbs.loop_for_vs(1),
transitions={'loop': 'START_VISUAL_SERVOING',
'continue': 'GRASP_OBJ'})
smach.StateMachine.add('START_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_start')]),
transitions={'success':'WAIT_VISUAL_SERVOING'})
smach.StateMachine.add('WAIT_VISUAL_SERVOING', gbs.wait_for_events([('/mcr_monitoring/component_wise_pose_error_monitor/event_out','e_stop', True)], timeout_duration=30),
transitions={'success':'STOP_VISUAL_SERVOING',
'timeout': 'STOP_VISUAL_SERVOING_WITH_FAILURE',
'failure':'STOP_VISUAL_SERVOING_WITH_FAILURE'})
smach.StateMachine.add('STOP_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_SUCCESS'})
smach.StateMachine.add('STOP_VISUAL_SERVOING_WITH_FAILURE', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_FAILURE'})
smach.StateMachine.add('SET_VISUAL_SERVOING_SUCCESS', mir_gbs.set_vs_status(status=True),
transitions={'success':'OPEN_GRIPPER_FOR_APPROACH_OBJECT'})
smach.StateMachine.add('SET_VISUAL_SERVOING_FAILURE', mir_gbs.set_vs_status(status=False),
transitions={'success':'SELECT_OBJECT_TO_BE_GRASPED'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_APPROACH_OBJECT', gms.control_gripper('open'),
transitions={'succeeded':'APPROACH_OBJECT'})
smach.StateMachine.add('APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_start')]),
transitions={'success':'WAIT_APPROACH_OBJECT'})
smach.StateMachine.add('WAIT_APPROACH_OBJECT', gbs.wait_for_events([('/guarded_approach_pose/coordinator/event_out','e_success', True)], timeout_duration=6),
transitions={'success':'STOP_APPROACH_OBJECT',
'timeout': 'STOP_APPROACH_OBJECT',
'failure':'STOP_APPROACH_OBJECT'})
smach.StateMachine.add('STOP_APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_stop')]),
transitions={'success':'CLOSE_GRIPPER'})
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM'})
smach.StateMachine.add('GRASP_OBJ', gms.linear_motion(operation='grasp'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('GRASP_OBJ_VISUAL_SERVOING', gms.linear_motion(operation='grasp', offset_x=-0.05),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('PRE_PLACE_OBJ_ON_REAR_PLATFORM', btts.pre_place_obj_on_rear_platform_btt(),
transitions={'succeeded':'VERIFY_GRASPED',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('VERIFY_GRASPED', gbs.send_event([('/gripper_controller/grasp_monitor/event_in', 'e_trigger')]),
transitions={'success':'WAIT_FOR_VERIFY_GRASPED'})
smach.StateMachine.add('WAIT_FOR_VERIFY_GRASPED', gbs.wait_for_events([('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5),
transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM',
'timeout':'OPEN_GRIPPER_FOR_GRASP_FAILURE',
'failure':'OPEN_GRIPPER_FOR_GRASP_FAILURE'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_GRASP_FAILURE', gms.control_gripper('open'),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS'})
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('DELETE_FROM_RECOGNIZED_OBJECTS', btts.delete_from_recognized_objects(),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'},
remapping={'object_to_delete': 'object_to_grasp'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_NEXT', gns.adjust_to_workspace(0.1),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED',
'failed':'ADJUST_POSE_WRT_WORKSPACE_NEXT'})
smach.StateMachine.add('NO_MORE_FREE_POSES_SAFE',gms.move_arm('look_at_workspace'),
transitions={'succeeded':'no_more_free_poses',
'failed':'SKIP_SOURCE_POSE_SAFE'})
# MISC STATES
smach.StateMachine.add('SKIP_SOURCE_POSE_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded':'SKIP_SOURCE_POSE',
'failed':'SKIP_SOURCE_POSE_SAFE'})
smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'),
transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK',
'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(
self,
outcomes=[
"pose_skipped_but_platform_limit_reached",
"no_more_free_poses",
"no_more_free_poses_at_robot_platf",
"no_more_task_for_given_type",
],
input_keys=[
"base_pose_to_approach",
"desired_distance_to_workspace",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_pose",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"recognized_objects",
"source_visits",
"task_list",
"test",
"vscount",
],
output_keys=[
"base_pose_to_approach",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"source_visits",
"task_list",
"test",
"vscount",
],
)
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add(
"SELECT_SOURCE_SUBTASK",
btts.select_btt_subtask(type="source"),
transitions={
"task_selected": "MOVE_TO_SOURCE_LOCATION_SAFE",
"no_more_task_for_given_type":
"no_more_task_for_given_type",
},
)
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "MOVE_TO_SOURCE_LOCATION",
"failed": "MOVE_TO_SOURCE_LOCATION_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION",
gns.approach_pose(),
transitions={
"succeeded": "STOP_ALL_COMPONENTS_AT_START",
"failed": "MOVE_TO_SOURCE_LOCATION",
},
)
smach.StateMachine.add(
"STOP_ALL_COMPONENTS_AT_START",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
gns.adjust_to_workspace(0.2),
transitions={
"succeeded": "SELECT_NEXT_LOOK_POSE",
"failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
},
)
smach.StateMachine.add(
"SELECT_NEXT_LOOK_POSE",
gms.select_arm_pose([
"look_at_workspace_right",
"look_at_workspace",
"look_at_workspace_left",
]),
transitions={
"succeeded": "LOOK_AROUND",
"failed": "RECOGNIZE_OBJECTS",
},
)
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm(),
transitions={
"succeeded": "RECOGNIZE_OBJECTS",
"failed": "LOOK_AROUND",
},
)
smach.StateMachine.add(
"RECOGNIZE_OBJECTS",
gps.find_objects(retries=1),
transitions={
"objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
"no_objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
},
remapping={"found_objects": "recognized_objects"},
)
smach.StateMachine.add(
"ACCUMULATE_RECOGNIZED_LISTS",
gps.accumulate_recognized_objects_list(),
transitions={
"complete": "SELECT_OBJECT_TO_BE_GRASPED",
"merged": "SELECT_NEXT_LOOK_POSE",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_TO_BE_GRASPED",
btts.select_object_to_be_grasped(),
transitions={
"obj_selected":
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"no_obj_selected":
"SKIP_SOURCE_POSE",
"no_more_free_poses_at_robot_platf":
"no_more_free_poses_at_robot_platf",
},
)
smach.StateMachine.add(
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
("/pregrasp_planner/event_in", "e_start"),
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_start",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_start",
),
]),
transitions={
"success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT"
},
)
smach.StateMachine.add(
"WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.wait_for_events([
("/pregrasp_planner/event_out", "e_success", True),
(
"/gripper_to_object_pose_error_calculator/event_out",
"e_success",
True,
),
(
"/mcr_common/relative_displacement_calculator/event_out",
"e_done",
True,
),
]),
transitions={
"success":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"timeout":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
"failure":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "MOVE_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"MOVE_ARM_AND_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_start",
),
("/planned_motion/event_in", "e_start"),
]),
transitions={"success": "WAIT_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"WAIT_ARM_AND_BASE_TO_OBJECT",
gbs.wait_for_events([
(
"/mcr_navigation/relative_base_controller/event_out",
"e_done",
True,
),
(
"/mcr_navigation/collision_velocity_filter/event_out",
"e_zero_velocities_forwarded",
False,
),
("/planned_motion/event_out", "e_success", True),
]),
transitions={
"success": "STOP_MOVE_ARM_BASE_TO_OBJECT",
"timeout": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={"success": "GRASP_OBJ"},
) # comment out if using visual servoing
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"ALIGN_WITH_OBJECT_LOOP",
mir_gbs.loop_for_vs(1),
transitions={
"loop": "START_VISUAL_SERVOING",
"continue": "GRASP_OBJ",
},
)
smach.StateMachine.add(
"START_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_start")]),
transitions={"success": "WAIT_VISUAL_SERVOING"},
)
smach.StateMachine.add(
"WAIT_VISUAL_SERVOING",
gbs.wait_for_events(
[(
"/mcr_monitoring/component_wise_pose_error_monitor/event_out",
"e_stop",
True,
)],
timeout_duration=30,
),
transitions={
"success": "STOP_VISUAL_SERVOING",
"timeout": "STOP_VISUAL_SERVOING_WITH_FAILURE",
"failure": "STOP_VISUAL_SERVOING_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_SUCCESS"},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING_WITH_FAILURE",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_FAILURE"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_SUCCESS",
mir_gbs.set_vs_status(status=True),
transitions={"success": "OPEN_GRIPPER_FOR_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_FAILURE",
mir_gbs.set_vs_status(status=False),
transitions={"success": "SELECT_OBJECT_TO_BE_GRASPED"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_APPROACH_OBJECT",
gms.control_gripper("open"),
transitions={"succeeded": "APPROACH_OBJECT"},
)
smach.StateMachine.add(
"APPROACH_OBJECT",
gbs.send_event([(
"/guarded_approach_pose/coordinator/event_in",
"e_start",
)]),
transitions={"success": "WAIT_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"WAIT_APPROACH_OBJECT",
gbs.wait_for_events(
[(
"/guarded_approach_pose/coordinator/event_out",
"e_success",
True,
)],
timeout_duration=6,
),
transitions={
"success": "STOP_APPROACH_OBJECT",
"timeout": "STOP_APPROACH_OBJECT",
"failure": "STOP_APPROACH_OBJECT",
},
)
smach.StateMachine.add(
"STOP_APPROACH_OBJECT",
gbs.send_event([("/guarded_approach_pose/coordinator/event_in",
"e_stop")]),
transitions={"success": "CLOSE_GRIPPER"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM"},
)
smach.StateMachine.add(
"GRASP_OBJ",
gms.linear_motion(operation="grasp"),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"GRASP_OBJ_VISUAL_SERVOING",
gms.linear_motion(operation="grasp", offset_x=-0.05),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"PRE_PLACE_OBJ_ON_REAR_PLATFORM",
btts.pre_place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "VERIFY_GRASPED",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"VERIFY_GRASPED",
gbs.send_event([(
"/gripper_controller/grasp_monitor/event_in",
"e_trigger",
)]),
transitions={"success": "WAIT_FOR_VERIFY_GRASPED"},
)
smach.StateMachine.add(
"WAIT_FOR_VERIFY_GRASPED",
gbs.wait_for_events(
[(
"/gripper_controller/grasp_monitor/event_out",
"e_object_grasped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "PLACE_OBJ_ON_REAR_PLATFORM",
"timeout": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
"failure": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_GRASP_FAILURE",
gms.control_gripper("open"),
transitions={"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add(
"PLACE_OBJ_ON_REAR_PLATFORM",
btts.place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"DELETE_FROM_RECOGNIZED_OBJECTS",
btts.delete_from_recognized_objects(),
transitions={"succeeded": "SELECT_OBJECT_TO_BE_GRASPED"},
remapping={"object_to_delete": "object_to_grasp"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_NEXT",
gns.adjust_to_workspace(0.1),
transitions={
"succeeded": "SELECT_OBJECT_TO_BE_GRASPED",
"failed": "ADJUST_POSE_WRT_WORKSPACE_NEXT",
},
)
smach.StateMachine.add(
"NO_MORE_FREE_POSES_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "no_more_free_poses",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
# MISC STATES
smach.StateMachine.add(
"SKIP_SOURCE_POSE_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "SKIP_SOURCE_POSE",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
smach.StateMachine.add(
"SKIP_SOURCE_POSE",
btts.skip_pose("source"),
transitions={
"pose_skipped":
"SELECT_SOURCE_SUBTASK",
"pose_skipped_but_platform_limit_reached":
"pose_skipped_but_platform_limit_reached",
},
)
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "failed", "no_object_for_ppt_platform"],
input_keys=[
"base_pose_to_approach",
"last_grasped_obj",
"move_arm_to",
"move_base_by",
"object_pose",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"selected_objects",
"next_arm_pose_index",
"cavity_pose",
"task_list",
],
output_keys=[
"base_pose_to_approach",
"last_grasped_obj",
"move_arm_to",
"move_base_by",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"selected_objects",
"next_arm_pose_index",
"cavity_pose",
"task_list",
],
)
with self:
# TODO: ENSURE WE HAVE ENOUGH SPACE IN THE ARENA
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
gns.adjust_to_workspace(0.2),
transitions={
"succeeded": "SELECT_OBJECTS_TO_PLACE",
"failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
},
)
# Select object from task list which needs to be place in cavities.
smach.StateMachine.add(
"SELECT_OBJECTS_TO_PLACE",
ppts.select_objects_to_place(),
transitions={
"objects_selected": "CLEAR_CAVITIES",
"no_more_obj_for_this_workspace":
"no_object_for_ppt_platform",
},
)
smach.StateMachine.add(
"CLEAR_CAVITIES",
ppts.clear_cavities(),
transitions={"succeeded": "SELECT_NEXT_LOOK_POSE"},
)
# CAVITY RECOGNITION PIPELINE
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": "CHECK_FOUND_CAVITIES",
},
)
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm(),
transitions={
"succeeded": "FIND_CAVITIES_LOOP",
"failed": "LOOK_AROUND",
},
)
smach.StateMachine.add(
"FIND_CAVITIES_LOOP",
gbs.loop_for(3),
transitions={
"loop": "FIND_CAVITIES",
"continue": "CHECK_FOUND_CAVITIES",
},
)
smach.StateMachine.add(
"FIND_CAVITIES",
gps.find_cavities(),
transitions={
"succeeded": "TRANSFORM_CAVITIES",
"failed": "SELECT_NEXT_LOOK_POSE",
},
)
smach.StateMachine.add(
"TRANSFORM_CAVITIES",
gps.transform_object_poses(frame_id="/odom"),
transitions={"succeeded": "FIND_BEST_MATCHED_CAVITIES"},
remapping={"found_objects": "found_cavities"},
)
smach.StateMachine.add(
"FIND_BEST_MATCHED_CAVITIES",
gps.find_best_matched_cavities(),
transitions={
"succeeded": "SELECT_NEXT_LOOK_POSE",
"complete": "CHECK_FOUND_CAVITIES",
},
)
smach.StateMachine.add(
"CHECK_FOUND_CAVITIES",
gps.check_found_cavities(),
transitions={
"cavities_found": "STOP_ALL_COMPONENTS_AT_START",
"no_cavities_found": "failed",
},
)
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"),
("/pregrasp_arm_monitor/event_in", "e_stop"),
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "SELECT_OBJECT_TO_PLACE"},
)
# OBJECT PLACING pipeline
# select cavitiy based on recognized cavities and list of objects to place in cavity.
smach.StateMachine.add(
"SELECT_OBJECT_TO_PLACE",
ppts.select_object_to_place(),
transitions={
"object_selected": "COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
"no_more_objects": "succeeded",
"no_more_cavities": "succeeded",
},
remapping={"found_objects": "best_matched_cavities"},
)
# ARM AND BASE ALIGNMENT TO CAVITY PIPELINE
smach.StateMachine.add(
"COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
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_CAVITY"
},
)
smach.StateMachine.add(
"WAIT_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
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_CAVITY",
"timeout":
"STOP_ALL_COMPONENTS", # should we try again for compute arm base shift
"failure": "STOP_ALL_COMPONENTS",
},
) # should we try again for compute arm base shift
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_CAVITY",
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": "ALIGN_BASE_WITH_CAVITY"},
)
smach.StateMachine.add(
"ALIGN_BASE_WITH_CAVITY",
gbs.send_event([(
"/mcr_navigation/relative_base_controller/event_in",
"e_start",
)]),
transitions={"success": "WAIT_ALIGN_BASE_WITH_CAVITY"},
)
smach.StateMachine.add(
"WAIT_ALIGN_BASE_WITH_CAVITY",
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,
),
]),
transitions={
"success": "STOP_ALIGN_BASE_WITH_CAVITY",
"timeout":
"STOP_ALL_COMPONENTS", # should we try again for compute and aligh arm and base to object
"failure": "STOP_ALL_COMPONENTS",
},
) # should we try again for compute and aligh arm and base to object
smach.StateMachine.add(
"STOP_ALIGN_BASE_WITH_CAVITY",
gbs.send_event([(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
)]),
transitions={"success": "GRASP_OBJECT_FOR_CAVITY_FROM_PLTF"},
)
# make sure that gripper is open
smach.StateMachine.add(
"GRASP_OBJECT_FOR_CAVITY_FROM_PLTF",
ppts.grasp_obj_for_hole_from_pltf(),
transitions={
"object_grasped": "MOVE_ARM_TO_CAVITY",
"no_more_obj_for_this_workspace":
"no_object_for_ppt_platform",
},
)
smach.StateMachine.add(
"GO_TO_PREGRASP",
gms.move_arm("pre_grasp"),
transitions={
"succeeded": "MOVE_ARM_TO_CAVITY",
"failed": "MOVE_ARM_TO_CAVITY",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_CAVITY",
gbs.send_event([
("/planned_motion/event_in", "e_start"),
# smach.StateMachine.add('MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in', 'e_start')]),
("/pregrasp_arm_monitor/event_in", "e_start"),
]),
transitions={"success": "WAIT_MOVE_ARM_TO_CAVITY"},
)
smach.StateMachine.add(
"WAIT_MOVE_ARM_TO_CAVITY",
gbs.wait_for_events([("/pregrasp_arm_monitor/event_out",
"e_done", True)]),
# smach.StateMachine.add('WAIT_MOVE_ARM_TO_CAVITY', gbs.wait_for_events([('/planned_motion/event_out','e_success', True)]),
# ('/pregrasp_arm_monitor/event_out', 'e_done', True)]),
transitions={
"success": "STOP_MOVE_ARM_TO_CAVITY",
"timeout":
"STOP_ALL_COMPONENTS", # should we try again for compute and aligh arm and base to object
"failure": "STOP_ALL_COMPONENTS",
},
) # should we try again for compute and aligh arm and base to object
# smach.StateMachine.add('STOP_MOVE_ARM_TO_CAVITY', gbs.send_event([('/planned_motion/event_in','e_stop')]),
smach.StateMachine.add(
"STOP_MOVE_ARM_TO_CAVITY",
gbs.send_event([
("/planned_motion/event_in", "e_stop"),
("/pregrasp_arm_monitor/event_in", "e_stop"),
]),
transitions={"success": "MOVE_ARM_TO_INTERMEDIATE_POSE"},
)
smach.StateMachine.add(
"PLACE_OBJECT",
gms.linear_motion(operation="release"),
transitions={
"succeeded": "WIGGLE_ARM_LEFT",
"failed": "PLACE_OBJECT",
},
)
smach.StateMachine.add(
"WIGGLE_ARM_LEFT",
ppts.ppt_wiggle_arm(wiggle_offset=-0.12),
transitions={
"succeeded": "WIGGLE_ARM_RIGHT",
"failed": "WIGGLE_ARM_RIGHT",
},
)
smach.StateMachine.add(
"WIGGLE_ARM_RIGHT",
ppts.ppt_wiggle_arm(wiggle_offset=0.24),
transitions={
"succeeded": "MOVE_ARM_TO_INTERMEDIATE_POSE",
"failed": "MOVE_ARM_TO_INTERMEDIATE_POSE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_INTERMEDIATE_POSE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "SELECT_OBJECT_TO_PLACE",
"failed": "MOVE_ARM_TO_INTERMEDIATE_POSE",
},
)
# normal placing if arm and base alignment pipeline fails
smach.StateMachine.add(
"STOP_ALL_COMPONENTS",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_arm_monitor/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "SELECT_OBJECT_TO_PLACE"},
)