def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded',
'no_more_obj_for_this_workspace'],
input_keys=['base_pose_to_approach',
'destinaton_free_poses',
'last_grasped_obj',
'move_arm_to',
'obj_goal_configuration_poses',
'objects_goal_configuration',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'task_list'],
output_keys=['base_pose_to_approach',
'destinaton_free_poses',
'last_grasped_obj',
'objects_goal_configuration',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'task_list'])
with self:
smach.StateMachine.add('ADD_WALLS_TO_PLANNING_SCENE', gms.update_static_elements_in_planning_scene("walls", "add"),
transitions={'succeeded':'GRASP_OBJECT_FROM_PLTF'})
smach.StateMachine.add('GRASP_OBJECT_FROM_PLTF', btts.grasp_obj_from_pltf_btt(),
transitions={'object_grasped':'REATTACH_OBJECT_TO_ROBOT',
'no_more_obj_for_this_workspace':'REMOVE_WALLS_FROM_PLANNING_SCENE'})
smach.StateMachine.add('REATTACH_OBJECT_TO_ROBOT', gms.update_robot_planning_scene("unload"),
transitions={'succeeded':'MOVE_TO_INTERMEDIATE_POSE'},
remapping={'object': 'last_grasped_obj'})
smach.StateMachine.add('MOVE_TO_INTERMEDIATE_POSE', gms.move_arm('platform_intermediate'),
transitions={'succeeded':'PLACE_OBJ_IN_CONFIGURATION',
'failed':'MOVE_TO_INTERMEDIATE_POSE'})
smach.StateMachine.add('PLACE_OBJ_IN_CONFIGURATION', btts.place_object_in_configuration_btt(),
transitions={'succeeded':'DELETE_OBJECT_FROM_ROBOT_1',
'no_more_cfg_poses':'DELETE_OBJECT_FROM_ROBOT_2'})
smach.StateMachine.add('DELETE_OBJECT_FROM_ROBOT_1', gms.update_robot_planning_scene("detach"),
transitions={'succeeded':'GRASP_OBJECT_FROM_PLTF'},
remapping={'object': 'last_grasped_obj'})
smach.StateMachine.add('DELETE_OBJECT_FROM_ROBOT_2', gms.update_robot_planning_scene("detach"),
transitions={'succeeded':'MOVE_ARM_INSIDE_BASE_BOUNDARIES'},
remapping={'object': 'object_to_grasp'})
#smach.StateMachine.add('AVOID_WALLS_PRE_3', gms.move_arm('candle'),
# transitions={'succeeded': 'MOVE_ARM_INSIDE_BASE_BOUNDARIES',
# 'failed': 'AVOID_WALLS_PRE_3'})
smach.StateMachine.add('MOVE_ARM_INSIDE_BASE_BOUNDARIES', gms.move_arm('platform_intermediate'),
transitions={'succeeded':'succeeded',
'failed':'MOVE_ARM_INSIDE_BASE_BOUNDARIES'})
smach.StateMachine.add('REMOVE_WALLS_FROM_PLANNING_SCENE', gms.update_static_elements_in_planning_scene("walls", "remove"),
transitions={'succeeded':'no_more_obj_for_this_workspace'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded',
'no_more_obj_for_this_workspace'],
input_keys=['base_pose_to_approach',
'destinaton_free_poses',
'last_grasped_obj',
'move_arm_to',
'obj_goal_configuration_poses',
'objects_goal_configuration',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'task_list'],
output_keys=['base_pose_to_approach',
'destinaton_free_poses',
'last_grasped_obj',
'objects_goal_configuration',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'task_list'])
with self:
smach.StateMachine.add('GRASP_OBJECT_FROM_PLTF', btts.grasp_obj_from_pltf_btt(),
transitions={'object_grasped':'PLACE_OBJ_IN_CONFIGURATION',
'no_more_obj_for_this_workspace':'no_more_obj_for_this_workspace'})
smach.StateMachine.add('PLACE_OBJ_IN_CONFIGURATION', btts.place_object_in_configuration_btt(),
transitions={'succeeded':'GRASP_OBJECT_FROM_PLTF',
'no_more_cfg_poses':'MOVE_ARM_INSIDE_BASE_BOUNDARIES'})
smach.StateMachine.add('MOVE_ARM_INSIDE_BASE_BOUNDARIES', gms.move_arm('look_at_workspace'),
transitions={'succeeded':'succeeded',
'failed':'MOVE_ARM_INSIDE_BASE_BOUNDARIES'})
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(self,
outcomes=['success', 'failure'],
input_keys=[
'move_arm_to', 'reference_point',
'end_effector_pose', 'benchmark_state',
'logging_status'
])
with self:
smach.StateMachine.add('MOVE_ARM_TO_CALIBRATION_CONFIGURATION',
gms.move_arm(),
transitions={
'succeeded': 'START_LOGGING',
'failed': 'START_LOGGING'
})
smach.StateMachine.add(
'START_LOGGING',
start_logging_offline_data(log_benchmark_data),
transitions={'success': 'STOP_COMPUTE_TASK_FRAME_TRANSFORM'})
smach.StateMachine.add(
'STOP_COMPUTE_TASK_FRAME_TRANSFORM',
gbs.send_and_wait_events_combined(
event_in_list=[('/compute_transform/event_in', 'e_stop')],
event_out_list=[('/compute_transform/event_out',
'e_stopped', True)],
timeout_duration=15),
transitions={
'success': 'PUBLISH_REFERENCE_POINT',
'timeout': 'PUBLISH_REFERENCE_POINT',
'failure': 'PUBLISH_REFERENCE_POINT'
})
smach.StateMachine.add(
'PUBLISH_REFERENCE_POINT',
cfs.publish_task_data(
topic_name='/compute_transform/reference_point'),
transitions={
'success': 'COMPUTE_TASK_FRAME_TRANSFORM',
'failure': 'failure'
},
remapping={'task_data': 'reference_point'})
smach.StateMachine.add(
'COMPUTE_TASK_FRAME_TRANSFORM',
gbs.send_and_wait_events_combined(
event_in_list=[('/compute_transform/event_in', 'e_start')],
event_out_list=[('/compute_transform/event_out',
'e_success', True)],
timeout_duration=15),
transitions={
'success': 'success',
'timeout': 'failure',
'failure': 'failure'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "no_more_obj_for_this_workspace"],
input_keys=[
"base_pose_to_approach",
"destinaton_free_poses",
"last_grasped_obj",
"move_arm_to",
"obj_goal_configuration_poses",
"objects_goal_configuration",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"task_list",
],
output_keys=[
"base_pose_to_approach",
"destinaton_free_poses",
"last_grasped_obj",
"objects_goal_configuration",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"task_list",
],
)
with self:
smach.StateMachine.add(
"GRASP_OBJECT_FROM_PLTF",
btts.grasp_obj_from_pltf_btt(),
transitions={
"object_grasped":
"PLACE_OBJ_IN_CONFIGURATION",
"no_more_obj_for_this_workspace":
"no_more_obj_for_this_workspace",
},
)
smach.StateMachine.add(
"PLACE_OBJ_IN_CONFIGURATION",
btts.place_object_in_configuration_btt(),
transitions={
"succeeded": "GRASP_OBJECT_FROM_PLTF",
"no_more_cfg_poses": "MOVE_ARM_INSIDE_BASE_BOUNDARIES",
},
)
smach.StateMachine.add(
"MOVE_ARM_INSIDE_BASE_BOUNDARIES",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "succeeded",
"failed": "MOVE_ARM_INSIDE_BASE_BOUNDARIES",
},
)
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):
smach.StateMachine.__init__(
self,
outcomes=['success', 'failure'],
output_keys=['is_object_grasped', 'end_effector_pose'])
with self:
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'LOOK_AT_WORKSPACE'})
smach.StateMachine.add('LOOK_AT_WORKSPACE',
gms.move_arm('look_at_workspace'),
transitions={
'succeeded': 'success',
'failed': 'failure'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["success", "failure"],
output_keys=["is_object_grasped", "end_effector_pose"],
)
with self:
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "LOOK_AT_WORKSPACE"},
)
smach.StateMachine.add(
"LOOK_AT_WORKSPACE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "success",
"failed": "failure"
},
)
def main():
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 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 __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['success',
'failure'])
with self:
#Opening the gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded':'STOP_POSE_SHIFTER'})
# Sending STOP to all components
smach.StateMachine.add('STOP_POSE_SHIFTER', gbs.send_event([('/plate_pose_shifter/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_POSE_SHIFT'})
smach.StateMachine.add('SET_NAMED_CONFIG_POSE_SHIFT', gbs.set_named_config('plate_pose_shifter'),
transitions={'success':'SHIFT_PLATE_POSE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_POSE_SHIFT'})
smach.StateMachine.add('SHIFT_PLATE_POSE', gbs.send_and_wait_events_combined(
event_in_list=[('/plate_pose_shifter/event_in','e_start')],
event_out_list=[('/plate_pose_shifter/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'SET_NAMED_CONFIG_PREGRASP',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('conveyor_belt_plate_pickup'),
transitions={'success':'PLAN_ARM_MOTION',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_PREGRASP'})
smach.StateMachine.add('PLAN_ARM_MOTION', gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in','e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out','e_success', True)]),
transitions={'success':'STOP_PLAN_ARM_MOTION',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in','e_stop')]),
transitions={'success':'MOVE_ARM_TO_OBJECT'})
smach.StateMachine.add('MOVE_ARM_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in','e_start')],
event_out_list=[('/move_arm_planned/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_APPROACH_PLATE'})
smach.StateMachine.add('SET_NAMED_CONFIG_APPROACH_PLATE', gbs.set_named_config('approach_plate'),
transitions={'success':'PLAN_APPROACH_PLATE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_APPROACH_PLATE'})
smach.StateMachine.add('PLAN_APPROACH_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')],
event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'APPROACH_PLATE',
'timeout':'APPROACH_PLATE',
'failure':'APPROACH_PLATE'})
smach.StateMachine.add('APPROACH_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/cartesian_controller_demo/event_in','e_start')],
event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'CLOSE_GRIPPER',
'timeout':'CLOSE_GRIPPER',
'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded':'VERIFY_GRIPPER_CLOSED'})
smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
timeout_duration=5),
transitions={'success':'STOP_APPROACH_PLATE',
'timeout':'CLOSE_GRIPPER',
'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('STOP_APPROACH_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'),
('/poses_to_move_wrtgripper/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_RETREAT_PLATE'})
smach.StateMachine.add('SET_NAMED_CONFIG_RETREAT_PLATE', gbs.set_named_config('retreat_plate'),
transitions={'success':'PLAN_RETREAT_PLATE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_RETREAT_PLATE'})
smach.StateMachine.add('PLAN_RETREAT_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')],
event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'RETREAT_PLATE',
'timeout':'RETREAT_PLATE',
'failure':'RETREAT_PLATE'})
smach.StateMachine.add('RETREAT_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/cartesian_controller_demo/event_in','e_start')],
event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_RETREAT_PLATE',
'timeout':'STOP_RETREAT_PLATE',
'failure':'STOP_RETREAT_PLATE'})
smach.StateMachine.add('STOP_RETREAT_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'),
('/poses_to_move_wrtgripper/event_in','e_stop')]),
transitions={'success':'MOVE_TO_HOLD'})
smach.StateMachine.add('MOVE_TO_HOLD', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'success',
'failed': 'failure'})
def main(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 = ['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('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('insert_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['goal'],
output_keys = ['feedback', 'result'])
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add('SELECT_OBJECT', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'SET_DBC_PARAMS',
'timeout':'UNSTAGE_OBJECT',
'failure':'UNSTAGE_OBJECT'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_try')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'STOP_POSE_SELECTOR',
'timeout':'STOP_POSE_SELECTOR',
'failure':'STOP_POSE_SELECTOR'})
smach.StateMachine.add('STOP_POSE_SELECTOR', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'PERCEIVE_CAVITY'})
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add('PERCEIVE_CAVITY', gas.perceive_cavity(),
transitions={'success': 'SELECT_OBJECT_AGAIN',
'failed' : 'OVERALL_FAILED'})
smach.StateMachine.add('SELECT_OBJECT_AGAIN', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'UNSTAGE_OBJECT',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(),
transitions={'success': 'TRY_INSERTING',
'failed' : 'OVERALL_FAILED'})
# execute robot motion
smach.StateMachine.add('TRY_INSERTING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start_arm_only')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=20),
transitions={'success':'OPEN_GRIPPER',
'timeout':'STAGE_OBJECT',
'failure':'STAGE_OBJECT'})
smach.StateMachine.add('STAGE_OBJECT', gas.stage_object(),
transitions={'success': 'OVERALL_FAILED',
'failed' : 'OVERALL_FAILED'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'WIGGLE_ARM'})
# wiggling the arm for precision placement
smach.StateMachine.add('WIGGLE_ARM', ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={'succeeded':'MOVE_ARM_TO_HOLD',
'failed':'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'STOP_POSE_SELECTOR_FINAL',
'failed':'MOVE_ARM_TO_HOLD'})
# sending e_stop to pose selector
smach.StateMachine.add('STOP_POSE_SELECTOR_FINAL', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'OVERALL_SUCCESS'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('insert_cavity_smach_viewer', sm, '/INSERT_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_cavity_server',
action_spec = GenericExecuteAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("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('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2'
})
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm_and_gripper('open_narrow'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
#TODO: verify if object is grasped or not
smach.StateMachine.add(
'SETUP_MOVE_ARM_PRE_STAGE_AGAIN',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN',
gms.move_arm(),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('smach_example_state_machine')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['insert_object_goal'],
output_keys=['insert_object_feedback', 'insert_object_result'])
with sm:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE',
send_event(
'/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'SET_TOP_HOLE_CONFIG'})
# reconfigure top hole shifter
smach.StateMachine.add('SET_TOP_HOLE_CONFIG',
gbs.set_named_config('top_hole_shifter'),
transitions={
'success': 'GENERATE_TOP_HOLE_POSE',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_CONFIG'
})
# receive a pose and offset's up, result: top hole pose (another pose)
smach.StateMachine.add(
'GENERATE_TOP_HOLE_POSE',
send_event('/top_hole_pose_shifter/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_CREATE_TOP_HOLE_POSE'})
# wait for pose shifter to give response
smach.StateMachine.add('WAIT_FOR_CREATE_TOP_HOLE_POSE',
wait_for_event(
'/top_hole_pose_shifter/event_out',
timeout_duration=5.0),
transitions={
'success': 'SET_TOP_HOLE_PREGRASP_CONFIG',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure top hole pregrasp params
smach.StateMachine.add('SET_TOP_HOLE_PREGRASP_CONFIG',
gbs.set_named_config('top_hole_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_PREGRASP_CONFIG'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add(
'PLAN_ARM_MOTION',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_ARM_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add(
'WAIT_PLAN_ARM_MOTION',
wait_for_event('/top_hole_pregrasp_planner_pipeline/event_out',
3.0),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
send_event('/top_hole_move_arm_planned/event_in', 'e_start'),
transitions={'success': 'WAIT_MOVE_ARM_TO_OBJECT'})
# wait for the arm motion to finish
smach.StateMachine.add('WAIT_MOVE_ARM_TO_OBJECT',
wait_for_event(
'/top_hole_move_arm_planned/event_out',
10.0),
transitions={
'success':
'STOP_MOVE_ARM_TO_OBJECT',
'timeout':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
'failure':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/top_hole_move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT',
send_event('/top_hole_move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'CONFIGURE_POSES_TO_MOVE_DOWN'})
# reconfigure the pose
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_DOWN',
gbs.set_named_config('move_down_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_DOWN',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_DOWN'
})
# linear motion down states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_DOWN',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_DOWN'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_DOWN',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_DOWN',
'failure': 'MOVE_ARM_TO_HOLD_FAILED',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_DOWN',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'OPEN_GRIPPER',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED',
'failure': 'MOVE_ARM_TO_HOLD_FAILED'
})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'OPEN_GRIPPER_AGAIN'})
# open gripper again (replace later on with padmaja stuff)
smach.StateMachine.add(
'OPEN_GRIPPER_AGAIN',
gms.control_gripper('open'),
transitions={'succeeded': 'CONFIGURE_POSES_TO_MOVE_UP'})
#reconfigure
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_UP',
gbs.set_named_config('move_up_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_UP',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_UP'
})
# linear motion up states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_UP',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_UP'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_UP',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_UP',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_UP',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_ARM_TO_HOLD',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD but provided that some old component failed
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILED',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_FAILURE',
'failed': 'MOVE_ARM_TO_HOLD_FAILED'
})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm,
'/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='insert_object_server',
action_spec=InsertObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='insert_object_goal',
feedback_key='insert_object_feedback',
result_key='insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['pick_object_goal'],
output_keys=['pick_object_feedback', 'pick_object_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object(
'/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
send_event('/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
wait_for_event('/mcr_perception/object_selector/event_out',
1.0),
transitions={
'success': 'RECONFIGURE_PREGRASP_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure pregrasp planner for pick
smach.StateMachine.add(
'RECONFIGURE_PREGRASP_PARAMS',
gbs.set_named_config('pick_normal_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'RECONFIGURE_PREGRASP_PARAMS'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose and wait for the result of the pregrasp planner
smach.StateMachine.add(
'PLAN_ARM_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in',
'e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_ARM_MOTION',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose and wait for its response
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in', 'e_start')],
event_out_list=[('/move_arm_planned/event_out', 'e_success',
True)],
timeout_duration=7),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT',
send_event('/move_arm_planned/event_in',
'e_stop'),
transitions={'success': 'CLOSE_GRIPPER'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# check if gripper changed in position, if not then close again
# smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
# event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
# event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
# timeout_duration=5),
# transitions={'success':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# optocoupler check if robot has a object in the gripper, if not then pick failed
# smach.StateMachine.add('VERIFY_OBJECT_GRASPED_OPTOCOUPLER', gbs.send_and_wait_events_combined(
# event_in_list=[('/optocoupler_gripper_status/event_in','e_trigger')],
# event_out_list=[('/optocoupler_gripper_status/event_out','e_grasped', True)],
# timeout_duration=5),
# transitions={'success':'SET_ACTION_LIB_SUCCESS',
# 'timeout':'SET_ACTION_LIB_FAILURE',
# 'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='pick_object_server',
action_spec=PickObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='pick_object_goal',
feedback_key='pick_object_feedback',
result_key='pick_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("insert_cavity_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "STOP_POSE_SELECTOR",
"timeout": "STOP_POSE_SELECTOR",
"failure": "STOP_POSE_SELECTOR",
},
)
smach.StateMachine.add(
"STOP_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "PERCEIVE_CAVITY"},
)
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add(
"PERCEIVE_CAVITY",
gas.perceive_cavity(),
transitions={
"success": "SELECT_OBJECT_AGAIN",
"failed": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_AGAIN",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "TRY_INSERTING",
"failed": "OVERALL_FAILED",
},
)
# execute robot motion
smach.StateMachine.add(
"TRY_INSERTING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start_arm_only")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STAGE_OBJECT",
"failure": "STAGE_OBJECT",
},
)
smach.StateMachine.add(
"STAGE_OBJECT",
gas.stage_object(),
transitions={
"success": "OVERALL_FAILED",
"failed": "OVERALL_FAILED",
},
)
# close gripper
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "WIGGLE_ARM"},
)
# wiggling the arm for precision placement
smach.StateMachine.add(
"WIGGLE_ARM",
ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={
"succeeded": "MOVE_ARM_TO_HOLD",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "STOP_POSE_SELECTOR_FINAL",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# sending e_stop to pose selector
smach.StateMachine.add(
"STOP_POSE_SELECTOR_FINAL",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "OVERALL_SUCCESS"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("insert_cavity_smach_viewer", sm,
"/INSERT_CAVITY_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_cavity_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("unstage_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
# read heavy object list
sm.userdata.heavy_objects = rospy.get_param("~heavy_objects", ["m20_100"])
with sm:
# add states to the container
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "CHECK_IF_OBJECT_HEAVY",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_HEAVY",
IsObjectHeavy(),
transitions={
"heavy": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
"light": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE",
"failed": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE",
"failed": "MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE",
SetupMoveArm("final"),
transitions={
"succeeded": "MOVE_ARM_STAGE",
"failed": "SETUP_MOVE_ARM_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE",
gms.move_arm(),
transitions={
"succeeded": "CLOSE_GRIPPER",
"failed": "MOVE_ARM_STAGE"
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
SetupMoveArm("pre", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_HEAVY",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_HEAVY",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE_HEAVY",
"failed": "MOVE_ARM_PRE_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE_HEAVY",
SetupMoveArm("final", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_STAGE_HEAVY",
"failed": "SETUP_MOVE_ARM_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE_HEAVY",
gms.move_arm(),
transitions={
"succeeded": "CLOSE_GRIPPER",
"failed": "MOVE_ARM_STAGE_HEAVY"
},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "CHECK_IF_OBJECT_HEAVY_AGAIN"},
)
# TODO: verify if object is grasped or not
smach.StateMachine.add(
"CHECK_IF_OBJECT_HEAVY_AGAIN",
IsObjectHeavy(),
transitions={
"heavy": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
"light": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_AGAIN",
gms.move_arm(blocking=True),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
SetupMoveArm("pre", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
gms.move_arm(blocking=False),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("unstage_object_smach_viewer", sm,
"/UNSTAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="unstage_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal', 'feedback'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('CHECK_IF_SHELF_INITIAL',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_PRE_PLACE_SHELF',
'not_shelf': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE_SHELF',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE_SHELF'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE',
gms.move_arm("15cm/pose4"),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'CHECK_IF_SHELF'})
smach.StateMachine.add('CHECK_IF_SHELF',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_SAFE_1',
'not_shelf': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_NEUTRAL',
gms.move_arm("barrier_tape"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_1',
gms.move_arm("shelf_intermediate_2"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_2',
'failed': 'MOVE_ARM_TO_SAFE_1'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_2',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_3',
'failed': 'MOVE_ARM_TO_SAFE_2'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_3',
gms.move_arm("platform_left_pre"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_SAFE_3'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('stage_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['stage_object_goal'],
output_keys = ['stage_object_feedback', 'stage_object_result'])
with sm:
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE', SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE',
'failed': 'SETUP_MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE', gms.move_arm(),
transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE', SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE',
'failed': 'SETUP_MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE', gms.move_arm(),
transitions={'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE_AGAIN', SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN',
'failed': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN', gms.move_arm(),
transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# smach viewer
sis = smach_ros.IntrospectionServer('stage_object_smach_viewer', sm, '/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'stage_object_server',
action_spec = StageObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'stage_object_goal',
feedback_key = 'stage_object_feedback',
result_key = 'stage_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("insert_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in",
"e_trigger")],
event_out_list=[(
"/mcr_perception/object_selector/event_out",
"e_selected",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "RE_GENERATE_OBJECT_POSE",
"failed": "OVERALL_FAILED",
},
)
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
"RE_GENERATE_OBJECT_POSE",
gbs.send_event([("/mcr_perception/object_selector/event_in",
"e_re_trigger")]),
transitions={"success": "MOVE_ROBOT_AND_PLACE"},
)
# execute robot motion
smach.StateMachine.add(
"MOVE_ROBOT_AND_PLACE",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "MOVE_ARM_TO_DEFAULT_PLACE",
"failure": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "MOVE_ARM_CARTESIAN",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_CARTESIAN",
gbs.send_and_wait_events_combined(
event_in_list=[("/insert_object_workaroud/event_in", "e_start")
],
event_out_list=[("/insert_object_workaroud/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STOP_ARM_CARTESIAN_MOTION",
"failure": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"STOP_ARM_CARTESIAN_MOTION",
gbs.send_event([("/insert_object_workaroud/event_in", "e_stop")]),
transitions={"success": "OPEN_GRIPPER"},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "MOVE_ARM_TO_HOLD"},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_HOLD",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("insert_object_smach_viewer", sm,
"/INSERT_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("perceive_cavity_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
"SETUP",
Setup(),
transitions={"succeeded": "PUBLISH_REFERENCE_FRAME_FOR_WBC"},
)
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
"PUBLISH_REFERENCE_FRAME_FOR_WBC",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_start")]),
transitions={"success": "START_POSE_SELECTOR"},
)
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
"START_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_start")]),
transitions={"success": "LOOK_AROUND"},
)
# move arm to selected pose
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm("look_at_workspace_from_near", blocking=True),
transitions={
"succeeded": "RECOGNIZE_CAVITIES",
"failed": "LOOK_AROUND",
},
)
# trigger perception pipeline
smach.StateMachine.add(
"RECOGNIZE_CAVITIES",
gps.find_cavities(retries=1),
transitions={
"cavities_found": "POPULATE_RESULT_WITH_CAVITIES",
"no_cavities_found": "OVERALL_FAILED",
},
)
# populate action server result with perceived objects
smach.StateMachine.add(
"POPULATE_RESULT_WITH_CAVITIES",
PopulateResultWithCavities(),
transitions={"succeeded": "OVERALL_SUCCESS"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("perceive_cavity_smach_viewer", sm,
"/PERCEIVE_CAVITY_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="perceive_cavity_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("place_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal", "feedback"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"CHECK_IF_SHELF_INITIAL",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_PRE_PLACE_SHELF",
"not_shelf": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE_SHELF",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE_SHELF",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"START_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_start")]),
transitions={"success": "GET_POSE_TO_PLACE_OBJECT"},
)
smach.StateMachine.add(
"GET_POSE_TO_PLACE_OBJECT",
GetPoseToPlaceOject(
"/mcr_perception/place_pose_selector/platform_name",
"/mcr_perception/place_pose_selector/place_pose",
"/mcr_perception/place_pose_selector/event_out",
15.0,
),
transitions={
"succeeded": "MOVE_ARM_TO_PLACE_OBJECT",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("15cm/pose4"),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PLACE_OBJECT",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "STOP_PLACE_POSE_SELECTOR"},
)
smach.StateMachine.add(
"STOP_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_stop")]),
transitions={"success": "CHECK_IF_SHELF"},
)
smach.StateMachine.add(
"CHECK_IF_SHELF",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_SAFE_1",
"not_shelf": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_NEUTRAL",
gms.move_arm("barrier_tape"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_1",
gms.move_arm("shelf_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_2",
"failed": "MOVE_ARM_TO_SAFE_1",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_2",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_3",
"failed": "MOVE_ARM_TO_SAFE_2",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_3",
gms.move_arm("platform_left_pre"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_SAFE_3",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("place_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="place_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __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 main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['unstage_object_goal'],
output_keys=['unstage_object_feedback', 'unstage_object_result'])
with sm:
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open_release'),
transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE'})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
# event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
# event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
# timeout_duration=5),
# transitions={'success':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# smach viewer
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=UnStageObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='unstage_object_goal',
feedback_key='unstage_object_feedback',
result_key='unstage_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['place_object_goal', 'place_object_feedback'],
output_keys=['place_object_feedback', 'place_object_result'])
with sm:
#add states to the container
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("navigation"),
transitions={'succeeded':'STOP_PLACE_POSE_SELECTOR',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=PlaceObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='place_object_goal',
feedback_key='place_object_feedback',
result_key='place_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("stage_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "SETUP_MOVE_ARM_PRE_STAGE",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE",
"failed": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE",
"failed": "MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE",
SetupMoveArm("final"),
transitions={
"succeeded": "MOVE_ARM_STAGE",
"failed": "SETUP_MOVE_ARM_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_STAGE"
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN"},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_AGAIN",
gms.move_arm(blocking=False),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("stage_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="stage_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('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()
