def execute(self, userdata):
# give feedback
feedback = PickObjectFeedback()
feedback.current_state = 'CBT_GRASP'
userdata.pick_object_feedback = feedback
gms_o.control_gripper('close')
#gms_o.gripper_command.go()
rospy.sleep(3.0)
return 'succeeded'
def execute(self, userdata):
#TODO TIMING
# give feedback
feedback = PickObjectFeedback()
feedback.current_state = 'CBT_RELEASE'
userdata.pick_object_feedback = feedback
gms_o.control_gripper("open")
#rospy.sleep(3.5)
#gms_o.gripper_command.set_named_target("open")
#gms_o.gripper_command.go()
rospy.sleep(2.0)
return 'succeeded'
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['success', 'failure'],
output_keys=['is_object_grasped', 'end_effector_pose'])
with self:
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'LOOK_AT_WORKSPACE'})
smach.StateMachine.add('LOOK_AT_WORKSPACE',
gms.move_arm('look_at_workspace'),
transitions={
'succeeded': 'success',
'failed': 'failure'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["success", "failure"],
output_keys=["is_object_grasped", "end_effector_pose"],
)
with self:
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "LOOK_AT_WORKSPACE"},
)
smach.StateMachine.add(
"LOOK_AT_WORKSPACE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "success",
"failed": "failure"
},
)
def main():
# Open the container
rospy.init_node("pick_object_wbc_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in",
"e_trigger")],
event_out_list=[(
"/mcr_perception/object_selector/event_out",
"e_selected",
True,
)],
timeout_duration=10,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "SET_DBC_PARAMS"},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_PICK",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
("/waypoint_trajectory_generation/event_in", "e_start"),
("/wbc/event_in", "e_stop"),
]),
transitions={"success": "OVERALL_FAILED"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE"},
)
# move arm to stage_intemediate position
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "VERIFY_OBJECT_GRASPED",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"VERIFY_OBJECT_GRASPED",
gbs.send_and_wait_events_combined(
event_in_list=[("/gripper_controller/grasp_monitor/event_in",
"e_trigger")],
event_out_list=[(
"/gripper_controller/grasp_monitor/event_out",
"e_object_grasped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("pick_object_smach_viewer", sm,
"/PICK_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="wbc_pick_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(
self,
outcomes=[
"pose_skipped_but_platform_limit_reached",
"no_more_free_poses",
"no_more_free_poses_at_robot_platf",
"no_more_task_for_given_type",
],
input_keys=[
"base_pose_to_approach",
"desired_distance_to_workspace",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_pose",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"recognized_objects",
"source_visits",
"task_list",
"test",
"vscount",
],
output_keys=[
"base_pose_to_approach",
"found_objects",
"lasttask",
"move_arm_to",
"move_base_by",
"next_arm_pose_index",
"object_to_be_adjust_to",
"object_to_grasp",
"objects_to_be_grasped",
"prev_vs_result",
"rear_platform_free_poses",
"rear_platform_occupied_poses",
"source_visits",
"task_list",
"test",
"vscount",
],
)
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add(
"SELECT_SOURCE_SUBTASK",
btts.select_btt_subtask(type="source"),
transitions={
"task_selected": "MOVE_TO_SOURCE_LOCATION_SAFE",
"no_more_task_for_given_type":
"no_more_task_for_given_type",
},
)
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "MOVE_TO_SOURCE_LOCATION",
"failed": "MOVE_TO_SOURCE_LOCATION_SAFE",
},
)
smach.StateMachine.add(
"MOVE_TO_SOURCE_LOCATION",
gns.approach_pose(),
transitions={
"succeeded": "STOP_ALL_COMPONENTS_AT_START",
"failed": "MOVE_TO_SOURCE_LOCATION",
},
)
smach.StateMachine.add(
"STOP_ALL_COMPONENTS_AT_START",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
gns.adjust_to_workspace(0.2),
transitions={
"succeeded": "SELECT_NEXT_LOOK_POSE",
"failed": "ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE",
},
)
smach.StateMachine.add(
"SELECT_NEXT_LOOK_POSE",
gms.select_arm_pose([
"look_at_workspace_right",
"look_at_workspace",
"look_at_workspace_left",
]),
transitions={
"succeeded": "LOOK_AROUND",
"failed": "RECOGNIZE_OBJECTS",
},
)
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm(),
transitions={
"succeeded": "RECOGNIZE_OBJECTS",
"failed": "LOOK_AROUND",
},
)
smach.StateMachine.add(
"RECOGNIZE_OBJECTS",
gps.find_objects(retries=1),
transitions={
"objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
"no_objects_found": "ACCUMULATE_RECOGNIZED_LISTS",
},
remapping={"found_objects": "recognized_objects"},
)
smach.StateMachine.add(
"ACCUMULATE_RECOGNIZED_LISTS",
gps.accumulate_recognized_objects_list(),
transitions={
"complete": "SELECT_OBJECT_TO_BE_GRASPED",
"merged": "SELECT_NEXT_LOOK_POSE",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_TO_BE_GRASPED",
btts.select_object_to_be_grasped(),
transitions={
"obj_selected":
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"no_obj_selected":
"SKIP_SOURCE_POSE",
"no_more_free_poses_at_robot_platf":
"no_more_free_poses_at_robot_platf",
},
)
smach.StateMachine.add(
"COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
("/pregrasp_planner/event_in", "e_start"),
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_start",
),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_start",
),
]),
transitions={
"success": "WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT"
},
)
smach.StateMachine.add(
"WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.wait_for_events([
("/pregrasp_planner/event_out", "e_success", True),
(
"/gripper_to_object_pose_error_calculator/event_out",
"e_success",
True,
),
(
"/mcr_common/relative_displacement_calculator/event_out",
"e_done",
True,
),
]),
transitions={
"success":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
"timeout":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
"failure":
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "MOVE_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/gripper_to_object_pose_error_calculator/event_in",
"e_stop",
),
("/pregrasp_planner/event_in", "e_stop"),
(
"/mcr_common/relative_displacement_calculator/event_in",
"e_stop",
),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"MOVE_ARM_AND_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_start",
),
("/planned_motion/event_in", "e_start"),
]),
transitions={"success": "WAIT_ARM_AND_BASE_TO_OBJECT"},
)
smach.StateMachine.add(
"WAIT_ARM_AND_BASE_TO_OBJECT",
gbs.wait_for_events([
(
"/mcr_navigation/relative_base_controller/event_out",
"e_done",
True,
),
(
"/mcr_navigation/collision_velocity_filter/event_out",
"e_zero_velocities_forwarded",
False,
),
("/planned_motion/event_out", "e_success", True),
]),
transitions={
"success": "STOP_MOVE_ARM_BASE_TO_OBJECT",
"timeout": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={"success": "GRASP_OBJ"},
) # comment out if using visual servoing
smach.StateMachine.add(
"STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
(
"/mcr_navigation/relative_base_controller/event_in",
"e_stop",
),
("/planned_motion/event_in", "e_stop"),
]),
transitions={"success": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
smach.StateMachine.add(
"ALIGN_WITH_OBJECT_LOOP",
mir_gbs.loop_for_vs(1),
transitions={
"loop": "START_VISUAL_SERVOING",
"continue": "GRASP_OBJ",
},
)
smach.StateMachine.add(
"START_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_start")]),
transitions={"success": "WAIT_VISUAL_SERVOING"},
)
smach.StateMachine.add(
"WAIT_VISUAL_SERVOING",
gbs.wait_for_events(
[(
"/mcr_monitoring/component_wise_pose_error_monitor/event_out",
"e_stop",
True,
)],
timeout_duration=30,
),
transitions={
"success": "STOP_VISUAL_SERVOING",
"timeout": "STOP_VISUAL_SERVOING_WITH_FAILURE",
"failure": "STOP_VISUAL_SERVOING_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_SUCCESS"},
)
smach.StateMachine.add(
"STOP_VISUAL_SERVOING_WITH_FAILURE",
gbs.send_event([("/mir_controllers/visual_servoing/event_in",
"e_stop")]),
transitions={"success": "SET_VISUAL_SERVOING_FAILURE"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_SUCCESS",
mir_gbs.set_vs_status(status=True),
transitions={"success": "OPEN_GRIPPER_FOR_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"SET_VISUAL_SERVOING_FAILURE",
mir_gbs.set_vs_status(status=False),
transitions={"success": "SELECT_OBJECT_TO_BE_GRASPED"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_APPROACH_OBJECT",
gms.control_gripper("open"),
transitions={"succeeded": "APPROACH_OBJECT"},
)
smach.StateMachine.add(
"APPROACH_OBJECT",
gbs.send_event([(
"/guarded_approach_pose/coordinator/event_in",
"e_start",
)]),
transitions={"success": "WAIT_APPROACH_OBJECT"},
)
smach.StateMachine.add(
"WAIT_APPROACH_OBJECT",
gbs.wait_for_events(
[(
"/guarded_approach_pose/coordinator/event_out",
"e_success",
True,
)],
timeout_duration=6,
),
transitions={
"success": "STOP_APPROACH_OBJECT",
"timeout": "STOP_APPROACH_OBJECT",
"failure": "STOP_APPROACH_OBJECT",
},
)
smach.StateMachine.add(
"STOP_APPROACH_OBJECT",
gbs.send_event([("/guarded_approach_pose/coordinator/event_in",
"e_stop")]),
transitions={"success": "CLOSE_GRIPPER"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM"},
)
smach.StateMachine.add(
"GRASP_OBJ",
gms.linear_motion(operation="grasp"),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"GRASP_OBJ_VISUAL_SERVOING",
gms.linear_motion(operation="grasp", offset_x=-0.05),
transitions={
"succeeded": "PRE_PLACE_OBJ_ON_REAR_PLATFORM",
"failed": "DELETE_FROM_RECOGNIZED_OBJECTS",
},
) # TODO: this could loop
smach.StateMachine.add(
"PRE_PLACE_OBJ_ON_REAR_PLATFORM",
btts.pre_place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "VERIFY_GRASPED",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"VERIFY_GRASPED",
gbs.send_event([(
"/gripper_controller/grasp_monitor/event_in",
"e_trigger",
)]),
transitions={"success": "WAIT_FOR_VERIFY_GRASPED"},
)
smach.StateMachine.add(
"WAIT_FOR_VERIFY_GRASPED",
gbs.wait_for_events(
[(
"/gripper_controller/grasp_monitor/event_out",
"e_object_grasped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "PLACE_OBJ_ON_REAR_PLATFORM",
"timeout": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
"failure": "OPEN_GRIPPER_FOR_GRASP_FAILURE",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_GRASP_FAILURE",
gms.control_gripper("open"),
transitions={"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS"},
)
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add(
"PLACE_OBJ_ON_REAR_PLATFORM",
btts.place_obj_on_rear_platform_btt(),
transitions={
"succeeded": "DELETE_FROM_RECOGNIZED_OBJECTS",
"no_more_free_poses": "no_more_free_poses",
},
)
smach.StateMachine.add(
"DELETE_FROM_RECOGNIZED_OBJECTS",
btts.delete_from_recognized_objects(),
transitions={"succeeded": "SELECT_OBJECT_TO_BE_GRASPED"},
remapping={"object_to_delete": "object_to_grasp"},
)
smach.StateMachine.add(
"ADJUST_POSE_WRT_WORKSPACE_NEXT",
gns.adjust_to_workspace(0.1),
transitions={
"succeeded": "SELECT_OBJECT_TO_BE_GRASPED",
"failed": "ADJUST_POSE_WRT_WORKSPACE_NEXT",
},
)
smach.StateMachine.add(
"NO_MORE_FREE_POSES_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "no_more_free_poses",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
# MISC STATES
smach.StateMachine.add(
"SKIP_SOURCE_POSE_SAFE",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "SKIP_SOURCE_POSE",
"failed": "SKIP_SOURCE_POSE_SAFE",
},
)
smach.StateMachine.add(
"SKIP_SOURCE_POSE",
btts.skip_pose("source"),
transitions={
"pose_skipped":
"SELECT_SOURCE_SUBTASK",
"pose_skipped_but_platform_limit_reached":
"pose_skipped_but_platform_limit_reached",
},
)
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(
self,
outcomes=['success', 'failure'],
input_keys=['is_object_grasped', 'end_effector_pose'],
output_keys=['is_object_grasped', 'end_effector_pose'])
with self:
smach.StateMachine.add(
'SET_NAMED_CONFIG_PREGRASP',
gbs.set_named_config('pregrasp_laying_object'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'failure',
'timeout': 'SET_NAMED_CONFIG_PREGRASP'
})
smach.StateMachine.add(
'PLAN_ARM_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in',
'e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out',
'e_success', True)]),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION',
gbs.send_event([('/pregrasp_planner_pipeline/event_in',
'e_stop')]),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in', 'e_start')],
event_out_list=[('/move_arm_planned/event_out',
'e_success', True)],
timeout_duration=15),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT',
gbs.send_event([('/move_arm_planned/event_in', 'e_stop')]),
transitions={'success': 'SAVE_GRIPPER_POSE'})
smach.StateMachine.add('SAVE_GRIPPER_POSE',
mfs.save_gripper_pose(),
transitions={'success': 'CLOSE_GRIPPER'})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'VERIFY_GRIPPER_CLOSED'})
smach.StateMachine.add('VERIFY_GRIPPER_CLOSED',
gbs.send_and_wait_events_combined(
event_in_list=[
('/gripper_state_monitor/event_in',
'e_trigger')
],
event_out_list=[
('/gripper_state_monitor/event_out',
'e_gripper_closed', True)
],
timeout_duration=5),
transitions={
'success':
'SET_NAMED_CONFIG_LIFT_OBJECT',
'timeout': 'CLOSE_GRIPPER',
'failure': 'CLOSE_GRIPPER'
})
smach.StateMachine.add('SET_NAMED_CONFIG_LIFT_OBJECT',
gbs.set_named_config('lift_object'),
transitions={
'success': 'PLAN_LIFT_OBJECT',
'failure': 'failure',
'timeout':
'SET_NAMED_CONFIG_LIFT_OBJECT'
})
smach.StateMachine.add('PLAN_LIFT_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[
('/poses_to_move_wrtbase/event_in',
'e_start')
],
event_out_list=[
('/poses_to_move_wrtbase/event_out',
'e_success', True)
],
timeout_duration=3),
transitions={
'success': 'LIFT_OBJECT',
'timeout': 'LIFT_OBJECT',
'failure': 'LIFT_OBJECT'
})
smach.StateMachine.add(
'LIFT_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/cartesian_controller_demo/event_in',
'e_start')],
event_out_list=[('/cartesian_controller_demo/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'VERIFY_GRASPED',
'timeout': 'VERIFY_GRASPED',
'failure': 'VERIFY_GRASPED'
})
smach.StateMachine.add(
'VERIFY_GRASPED',
gbs.send_and_wait_events_combined(
event_in_list=[
('/gripper_controller/grasp_monitor/event_in',
'e_trigger')
],
event_out_list=[
('/gripper_controller/grasp_monitor/event_out',
'e_object_grasped', True)
],
timeout_duration=10),
transitions={
'success': 'SET_OBJECT_GRASPED',
'timeout': 'SET_OBJECT_NOT_GRASPED',
'failure': 'SET_OBJECT_NOT_GRASPED'
})
smach.StateMachine.add('SET_OBJECT_GRASPED',
mfs.set_is_object_grasped(True),
transitions={'success': 'success'})
smach.StateMachine.add('SET_OBJECT_NOT_GRASPED',
mfs.set_is_object_grasped(False),
transitions={'success': 'success'})
def main():
rospy.init_node('smach_example_state_machine')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['insert_object_goal'],
output_keys=['insert_object_feedback', 'insert_object_result'])
with sm:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE',
send_event(
'/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'SET_TOP_HOLE_CONFIG'})
# reconfigure top hole shifter
smach.StateMachine.add('SET_TOP_HOLE_CONFIG',
gbs.set_named_config('top_hole_shifter'),
transitions={
'success': 'GENERATE_TOP_HOLE_POSE',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_CONFIG'
})
# receive a pose and offset's up, result: top hole pose (another pose)
smach.StateMachine.add(
'GENERATE_TOP_HOLE_POSE',
send_event('/top_hole_pose_shifter/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_CREATE_TOP_HOLE_POSE'})
# wait for pose shifter to give response
smach.StateMachine.add('WAIT_FOR_CREATE_TOP_HOLE_POSE',
wait_for_event(
'/top_hole_pose_shifter/event_out',
timeout_duration=5.0),
transitions={
'success': 'SET_TOP_HOLE_PREGRASP_CONFIG',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure top hole pregrasp params
smach.StateMachine.add('SET_TOP_HOLE_PREGRASP_CONFIG',
gbs.set_named_config('top_hole_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_PREGRASP_CONFIG'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add(
'PLAN_ARM_MOTION',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_ARM_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add(
'WAIT_PLAN_ARM_MOTION',
wait_for_event('/top_hole_pregrasp_planner_pipeline/event_out',
3.0),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/top_hole_pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
send_event('/top_hole_move_arm_planned/event_in', 'e_start'),
transitions={'success': 'WAIT_MOVE_ARM_TO_OBJECT'})
# wait for the arm motion to finish
smach.StateMachine.add('WAIT_MOVE_ARM_TO_OBJECT',
wait_for_event(
'/top_hole_move_arm_planned/event_out',
10.0),
transitions={
'success':
'STOP_MOVE_ARM_TO_OBJECT',
'timeout':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
'failure':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/top_hole_move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT',
send_event('/top_hole_move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'CONFIGURE_POSES_TO_MOVE_DOWN'})
# reconfigure the pose
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_DOWN',
gbs.set_named_config('move_down_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_DOWN',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_DOWN'
})
# linear motion down states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_DOWN',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_DOWN'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_DOWN',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_DOWN',
'failure': 'MOVE_ARM_TO_HOLD_FAILED',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_DOWN',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'OPEN_GRIPPER',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED',
'failure': 'MOVE_ARM_TO_HOLD_FAILED'
})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'OPEN_GRIPPER_AGAIN'})
# open gripper again (replace later on with padmaja stuff)
smach.StateMachine.add(
'OPEN_GRIPPER_AGAIN',
gms.control_gripper('open'),
transitions={'succeeded': 'CONFIGURE_POSES_TO_MOVE_UP'})
#reconfigure
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_UP',
gbs.set_named_config('move_up_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_UP',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_UP'
})
# linear motion up states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_UP',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_UP'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_UP',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_UP',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_UP',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_ARM_TO_HOLD',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD but provided that some old component failed
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILED',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_FAILURE',
'failed': 'MOVE_ARM_TO_HOLD_FAILED'
})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm,
'/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='insert_object_server',
action_spec=InsertObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='insert_object_goal',
feedback_key='insert_object_feedback',
result_key='insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('insert_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['goal'],
output_keys = ['feedback', 'result'])
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add('SELECT_OBJECT', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'SET_DBC_PARAMS',
'timeout':'UNSTAGE_OBJECT',
'failure':'UNSTAGE_OBJECT'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_try')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'STOP_POSE_SELECTOR',
'timeout':'STOP_POSE_SELECTOR',
'failure':'STOP_POSE_SELECTOR'})
smach.StateMachine.add('STOP_POSE_SELECTOR', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'PERCEIVE_CAVITY'})
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add('PERCEIVE_CAVITY', gas.perceive_cavity(),
transitions={'success': 'SELECT_OBJECT_AGAIN',
'failed' : 'OVERALL_FAILED'})
smach.StateMachine.add('SELECT_OBJECT_AGAIN', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'UNSTAGE_OBJECT',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(),
transitions={'success': 'TRY_INSERTING',
'failed' : 'OVERALL_FAILED'})
# execute robot motion
smach.StateMachine.add('TRY_INSERTING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start_arm_only')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=20),
transitions={'success':'OPEN_GRIPPER',
'timeout':'STAGE_OBJECT',
'failure':'STAGE_OBJECT'})
smach.StateMachine.add('STAGE_OBJECT', gas.stage_object(),
transitions={'success': 'OVERALL_FAILED',
'failed' : 'OVERALL_FAILED'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'WIGGLE_ARM'})
# wiggling the arm for precision placement
smach.StateMachine.add('WIGGLE_ARM', ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={'succeeded':'MOVE_ARM_TO_HOLD',
'failed':'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'STOP_POSE_SELECTOR_FINAL',
'failed':'MOVE_ARM_TO_HOLD'})
# sending e_stop to pose selector
smach.StateMachine.add('STOP_POSE_SELECTOR_FINAL', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'OVERALL_SUCCESS'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('insert_cavity_smach_viewer', sm, '/INSERT_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_cavity_server',
action_spec = GenericExecuteAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal', 'feedback'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('CHECK_IF_SHELF_INITIAL',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_PRE_PLACE_SHELF',
'not_shelf': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE_SHELF',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE_SHELF'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE',
gms.move_arm("15cm/pose4"),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'CHECK_IF_SHELF'})
smach.StateMachine.add('CHECK_IF_SHELF',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_SAFE_1',
'not_shelf': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_NEUTRAL',
gms.move_arm("barrier_tape"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_1',
gms.move_arm("shelf_intermediate_2"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_2',
'failed': 'MOVE_ARM_TO_SAFE_1'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_2',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_3',
'failed': 'MOVE_ARM_TO_SAFE_2'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_3',
gms.move_arm("platform_left_pre"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_SAFE_3'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("place_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal", "feedback"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"CHECK_IF_SHELF_INITIAL",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_PRE_PLACE_SHELF",
"not_shelf": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE_SHELF",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE_SHELF",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"START_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_start")]),
transitions={"success": "GET_POSE_TO_PLACE_OBJECT"},
)
smach.StateMachine.add(
"GET_POSE_TO_PLACE_OBJECT",
GetPoseToPlaceOject(
"/mcr_perception/place_pose_selector/platform_name",
"/mcr_perception/place_pose_selector/place_pose",
"/mcr_perception/place_pose_selector/event_out",
15.0,
),
transitions={
"succeeded": "MOVE_ARM_TO_PLACE_OBJECT",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("15cm/pose4"),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PLACE_OBJECT",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "STOP_PLACE_POSE_SELECTOR"},
)
smach.StateMachine.add(
"STOP_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_stop")]),
transitions={"success": "CHECK_IF_SHELF"},
)
smach.StateMachine.add(
"CHECK_IF_SHELF",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_SAFE_1",
"not_shelf": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_NEUTRAL",
gms.move_arm("barrier_tape"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_1",
gms.move_arm("shelf_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_2",
"failed": "MOVE_ARM_TO_SAFE_1",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_2",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_3",
"failed": "MOVE_ARM_TO_SAFE_2",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_3",
gms.move_arm("platform_left_pre"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_SAFE_3",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("place_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="place_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['place_object_goal', 'place_object_feedback'],
output_keys=['place_object_feedback', 'place_object_result'])
with sm:
#add states to the container
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("navigation"),
transitions={'succeeded':'STOP_PLACE_POSE_SELECTOR',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=PlaceObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='place_object_goal',
feedback_key='place_object_feedback',
result_key='place_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['success',
'failure'])
with self:
#Opening the gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded':'STOP_POSE_SHIFTER'})
# Sending STOP to all components
smach.StateMachine.add('STOP_POSE_SHIFTER', gbs.send_event([('/plate_pose_shifter/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_POSE_SHIFT'})
smach.StateMachine.add('SET_NAMED_CONFIG_POSE_SHIFT', gbs.set_named_config('plate_pose_shifter'),
transitions={'success':'SHIFT_PLATE_POSE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_POSE_SHIFT'})
smach.StateMachine.add('SHIFT_PLATE_POSE', gbs.send_and_wait_events_combined(
event_in_list=[('/plate_pose_shifter/event_in','e_start')],
event_out_list=[('/plate_pose_shifter/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'SET_NAMED_CONFIG_PREGRASP',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('conveyor_belt_plate_pickup'),
transitions={'success':'PLAN_ARM_MOTION',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_PREGRASP'})
smach.StateMachine.add('PLAN_ARM_MOTION', gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in','e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out','e_success', True)]),
transitions={'success':'STOP_PLAN_ARM_MOTION',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in','e_stop')]),
transitions={'success':'MOVE_ARM_TO_OBJECT'})
smach.StateMachine.add('MOVE_ARM_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in','e_start')],
event_out_list=[('/move_arm_planned/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT',
'timeout':'failure',
'failure':'failure'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_APPROACH_PLATE'})
smach.StateMachine.add('SET_NAMED_CONFIG_APPROACH_PLATE', gbs.set_named_config('approach_plate'),
transitions={'success':'PLAN_APPROACH_PLATE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_APPROACH_PLATE'})
smach.StateMachine.add('PLAN_APPROACH_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')],
event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'APPROACH_PLATE',
'timeout':'APPROACH_PLATE',
'failure':'APPROACH_PLATE'})
smach.StateMachine.add('APPROACH_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/cartesian_controller_demo/event_in','e_start')],
event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'CLOSE_GRIPPER',
'timeout':'CLOSE_GRIPPER',
'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded':'VERIFY_GRIPPER_CLOSED'})
smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
timeout_duration=5),
transitions={'success':'STOP_APPROACH_PLATE',
'timeout':'CLOSE_GRIPPER',
'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('STOP_APPROACH_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'),
('/poses_to_move_wrtgripper/event_in','e_stop')]),
transitions={'success':'SET_NAMED_CONFIG_RETREAT_PLATE'})
smach.StateMachine.add('SET_NAMED_CONFIG_RETREAT_PLATE', gbs.set_named_config('retreat_plate'),
transitions={'success':'PLAN_RETREAT_PLATE',
'failure':'failure',
'timeout': 'SET_NAMED_CONFIG_RETREAT_PLATE'})
smach.StateMachine.add('PLAN_RETREAT_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')],
event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'RETREAT_PLATE',
'timeout':'RETREAT_PLATE',
'failure':'RETREAT_PLATE'})
smach.StateMachine.add('RETREAT_PLATE', gbs.send_and_wait_events_combined(
event_in_list=[('/cartesian_controller_demo/event_in','e_start')],
event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_RETREAT_PLATE',
'timeout':'STOP_RETREAT_PLATE',
'failure':'STOP_RETREAT_PLATE'})
smach.StateMachine.add('STOP_RETREAT_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'),
('/poses_to_move_wrtgripper/event_in','e_stop')]),
transitions={'success':'MOVE_TO_HOLD'})
smach.StateMachine.add('MOVE_TO_HOLD', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'success',
'failed': 'failure'})
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=["success", "failure"])
with self:
# Opening the gripper
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "STOP_POSE_SHIFTER"},
)
# Sending STOP to all components
smach.StateMachine.add(
"STOP_POSE_SHIFTER",
gbs.send_event([("/plate_pose_shifter/event_in", "e_stop")]),
transitions={"success": "SET_NAMED_CONFIG_POSE_SHIFT"},
)
smach.StateMachine.add(
"SET_NAMED_CONFIG_POSE_SHIFT",
gbs.set_named_config("plate_pose_shifter"),
transitions={
"success": "SHIFT_PLATE_POSE",
"failure": "failure",
"timeout": "SET_NAMED_CONFIG_POSE_SHIFT",
},
)
smach.StateMachine.add(
"SHIFT_PLATE_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/plate_pose_shifter/event_in", "e_start")
],
event_out_list=[("/plate_pose_shifter/event_out",
"e_success", True)],
timeout_duration=5,
),
transitions={
"success": "SET_NAMED_CONFIG_PREGRASP",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"SET_NAMED_CONFIG_PREGRASP",
gbs.set_named_config("conveyor_belt_plate_pickup"),
transitions={
"success": "PLAN_ARM_MOTION",
"failure": "failure",
"timeout": "SET_NAMED_CONFIG_PREGRASP",
},
)
smach.StateMachine.add(
"PLAN_ARM_MOTION",
gbs.send_and_wait_events_combined(
event_in_list=[("/pregrasp_planner_pipeline/event_in",
"e_start")],
event_out_list=[("/pregrasp_planner_pipeline/event_out",
"e_success", True)],
),
transitions={
"success": "STOP_PLAN_ARM_MOTION",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"STOP_PLAN_ARM_MOTION",
gbs.send_event([("/pregrasp_planner_pipeline/event_in",
"e_stop")]),
transitions={"success": "MOVE_ARM_TO_OBJECT"},
)
smach.StateMachine.add(
"MOVE_ARM_TO_OBJECT",
gbs.send_and_wait_events_combined(
event_in_list=[("/move_arm_planned/event_in", "e_start")],
event_out_list=[("/move_arm_planned/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "STOP_MOVE_ARM_TO_OBJECT",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"STOP_MOVE_ARM_TO_OBJECT",
gbs.send_event([("/move_arm_planned/event_in", "e_stop")]),
transitions={"success": "SET_NAMED_CONFIG_APPROACH_PLATE"},
)
smach.StateMachine.add(
"SET_NAMED_CONFIG_APPROACH_PLATE",
gbs.set_named_config("approach_plate"),
transitions={
"success": "PLAN_APPROACH_PLATE",
"failure": "failure",
"timeout": "SET_NAMED_CONFIG_APPROACH_PLATE",
},
)
smach.StateMachine.add(
"PLAN_APPROACH_PLATE",
gbs.send_and_wait_events_combined(
event_in_list=[("/poses_to_move_wrtgripper/event_in",
"e_start")],
event_out_list=[("/poses_to_move_wrtgripper/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "APPROACH_PLATE",
"timeout": "APPROACH_PLATE",
"failure": "APPROACH_PLATE",
},
)
smach.StateMachine.add(
"APPROACH_PLATE",
gbs.send_and_wait_events_combined(
event_in_list=[("/cartesian_controller_demo/event_in",
"e_start")],
event_out_list=[("/cartesian_controller_demo/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "CLOSE_GRIPPER",
"failure": "CLOSE_GRIPPER",
},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "VERIFY_GRIPPER_CLOSED"},
)
smach.StateMachine.add(
"VERIFY_GRIPPER_CLOSED",
gbs.send_and_wait_events_combined(
event_in_list=[("/gripper_state_monitor/event_in",
"e_trigger")],
event_out_list=[("/gripper_state_monitor/event_out",
"e_gripper_closed", True)],
timeout_duration=5,
),
transitions={
"success": "STOP_APPROACH_PLATE",
"timeout": "CLOSE_GRIPPER",
"failure": "CLOSE_GRIPPER",
},
)
smach.StateMachine.add(
"STOP_APPROACH_PLATE",
gbs.send_event([
("/cartesian_controller_demo/event_in", "e_stop"),
("/poses_to_move_wrtgripper/event_in", "e_stop"),
]),
transitions={"success": "SET_NAMED_CONFIG_RETREAT_PLATE"},
)
smach.StateMachine.add(
"SET_NAMED_CONFIG_RETREAT_PLATE",
gbs.set_named_config("retreat_plate"),
transitions={
"success": "PLAN_RETREAT_PLATE",
"failure": "failure",
"timeout": "SET_NAMED_CONFIG_RETREAT_PLATE",
},
)
smach.StateMachine.add(
"PLAN_RETREAT_PLATE",
gbs.send_and_wait_events_combined(
event_in_list=[("/poses_to_move_wrtgripper/event_in",
"e_start")],
event_out_list=[("/poses_to_move_wrtgripper/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "RETREAT_PLATE",
"timeout": "RETREAT_PLATE",
"failure": "RETREAT_PLATE",
},
)
smach.StateMachine.add(
"RETREAT_PLATE",
gbs.send_and_wait_events_combined(
event_in_list=[("/cartesian_controller_demo/event_in",
"e_start")],
event_out_list=[("/cartesian_controller_demo/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "STOP_RETREAT_PLATE",
"timeout": "STOP_RETREAT_PLATE",
"failure": "STOP_RETREAT_PLATE",
},
)
smach.StateMachine.add(
"STOP_RETREAT_PLATE",
gbs.send_event([
("/cartesian_controller_demo/event_in", "e_stop"),
("/poses_to_move_wrtgripper/event_in", "e_stop"),
]),
transitions={"success": "MOVE_TO_HOLD"},
)
smach.StateMachine.add(
"MOVE_TO_HOLD",
gms.move_arm("look_at_workspace"),
transitions={
"succeeded": "success",
"failed": "failure"
},
)
def main():
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
smach.StateMachine.add(
'SELECT_OBJECT',
SelectObject('/mcr_perception/object_selector/input/object_name'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose of object
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_selector/event_in',
'e_trigger')],
event_out_list=[('/mcr_perception/object_selector/event_out',
'e_selected', True)],
timeout_duration=10),
transitions={
'success': 'OPEN_GRIPPER',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'SET_DBC_PARAMS'})
smach.StateMachine.add('SET_DBC_PARAMS',
gbs.set_named_config('dbc_pick_object'),
transitions={
'success': 'MOVE_ROBOT_AND_PICK',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add('MOVE_ROBOT_AND_PICK',
gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in', 'e_start')
],
event_out_list=[('/wbc/event_out',
'e_success', True)],
timeout_duration=50),
transitions={
'success':
'CLOSE_GRIPPER',
'timeout':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
gbs.send_event([
('/waypoint_trajectory_generation/event_in',
'e_start'), ('/wbc/event_in', 'e_stop')
]),
transitions={'success': 'OVERALL_FAILED'})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'})
# move arm to stage_intemediate position
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'VERIFY_OBJECT_GRASPED',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add(
'VERIFY_OBJECT_GRASPED',
gbs.send_and_wait_events_combined(
event_in_list=[('/gripper_controller/grasp_monitor/event_in',
'e_trigger')],
event_out_list=[('/gripper_controller/grasp_monitor/event_out',
'e_object_grasped', True)],
timeout_duration=5),
transitions={
'success': 'OVERALL_SUCCESS',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='wbc_pick_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self, use_mockup=None):
smach.StateMachine.__init__(self, outcomes=['pose_skipped_but_platform_limit_reached',
'no_more_free_poses',
'no_more_free_poses_at_robot_platf',
'no_more_task_for_given_type'],
input_keys=['base_pose_to_approach',
'desired_distance_to_workspace',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_pose',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'recognized_objects',
'source_visits',
'task_list',
'test',
'vscount'],
output_keys=['base_pose_to_approach',
'found_objects',
'lasttask',
'move_arm_to',
'move_base_by',
'next_arm_pose_index',
'object_to_be_adjust_to',
'object_to_grasp',
'objects_to_be_grasped',
'prev_vs_result',
'rear_platform_free_poses',
'rear_platform_occupied_poses',
'source_visits',
'task_list',
'test',
'vscount'])
self.use_mockup = use_mockup
self.use_visual_servoing = True
with self:
smach.StateMachine.add('SELECT_SOURCE_SUBTASK', btts.select_btt_subtask(type="source"),
transitions={'task_selected': 'MOVE_TO_SOURCE_LOCATION_SAFE',
'no_more_task_for_given_type': 'no_more_task_for_given_type'})
# required before any call to gns.approach_pose, moves arm within footprint
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded': 'MOVE_TO_SOURCE_LOCATION',
'failed': 'MOVE_TO_SOURCE_LOCATION_SAFE'})
smach.StateMachine.add('MOVE_TO_SOURCE_LOCATION', gns.approach_pose(),
transitions={'succeeded': 'STOP_ALL_COMPONENTS_AT_START',
'failed': 'MOVE_TO_SOURCE_LOCATION'})
smach.StateMachine.add('STOP_ALL_COMPONENTS_AT_START', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE', gns.adjust_to_workspace(0.2),
transitions={'succeeded':'SELECT_NEXT_LOOK_POSE',
'failed':'ADJUST_POSE_WRT_WORKSPACE_AT_SOURCE'})
smach.StateMachine.add('SELECT_NEXT_LOOK_POSE', gms.select_arm_pose(['look_at_workspace_right', 'look_at_workspace','look_at_workspace_left']),
transitions={'succeeded': 'LOOK_AROUND',
'failed': 'RECOGNIZE_OBJECTS'})
smach.StateMachine.add('LOOK_AROUND', gms.move_arm(),
transitions={'succeeded': 'RECOGNIZE_OBJECTS',
'failed': 'LOOK_AROUND'})
smach.StateMachine.add('RECOGNIZE_OBJECTS', gps.find_objects(retries=1),
transitions={'objects_found': 'ACCUMULATE_RECOGNIZED_LISTS',
'no_objects_found':'ACCUMULATE_RECOGNIZED_LISTS'},
remapping={'found_objects':'recognized_objects'})
smach.StateMachine.add('ACCUMULATE_RECOGNIZED_LISTS', gps.accumulate_recognized_objects_list(),
transitions={'complete': 'SELECT_OBJECT_TO_BE_GRASPED',
'merged':'SELECT_NEXT_LOOK_POSE'})
smach.StateMachine.add('SELECT_OBJECT_TO_BE_GRASPED', btts.select_object_to_be_grasped(),
transitions={'obj_selected':'COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'no_obj_selected':'SKIP_SOURCE_POSE',
'no_more_free_poses_at_robot_platf':'no_more_free_poses_at_robot_platf'})
smach.StateMachine.add('COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/pregrasp_planner/event_in','e_start'),
('/gripper_to_object_pose_error_calculator/event_in','e_start'),
('/mcr_common/relative_displacement_calculator/event_in','e_start')]),
transitions={'success':'WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT'})
smach.StateMachine.add('WAIT_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.wait_for_events([('/pregrasp_planner/event_out','e_success', True),
('/gripper_to_object_pose_error_calculator/event_out','e_success', True),
('/mcr_common/relative_displacement_calculator/event_out','e_done', True)]),
transitions={'success':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT',
'timeout': 'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'MOVE_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('STOP_COMPUTE_ARM_BASE_SHIFT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/gripper_to_object_pose_error_calculator/event_in','e_stop'),
('/pregrasp_planner/event_in','e_stop'),
('/mcr_common/relative_displacement_calculator/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('MOVE_ARM_AND_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_start'),
('/planned_motion/event_in', 'e_start')]),
transitions={'success':'WAIT_ARM_AND_BASE_TO_OBJECT'})
smach.StateMachine.add('WAIT_ARM_AND_BASE_TO_OBJECT', gbs.wait_for_events([('/mcr_navigation/relative_base_controller/event_out','e_done', True),
('/mcr_navigation/collision_velocity_filter/event_out','e_zero_velocities_forwarded', False),
('/planned_motion/event_out', 'e_success', True)]),
transitions={'success':'STOP_MOVE_ARM_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
# transitions={'success':'ALIGN_WITH_OBJECT_LOOP'}) # uncomment to use visual servoing
transitions={'success':'GRASP_OBJ'}) # comment out if using visual servoing
smach.StateMachine.add('STOP_MOVE_ARM_BASE_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/mcr_navigation/relative_base_controller/event_in','e_stop'),
('/planned_motion/event_in','e_stop')]),
transitions={'success':'DELETE_FROM_RECOGNIZED_OBJECTS'})
smach.StateMachine.add('ALIGN_WITH_OBJECT_LOOP', mir_gbs.loop_for_vs(1),
transitions={'loop': 'START_VISUAL_SERVOING',
'continue': 'GRASP_OBJ'})
smach.StateMachine.add('START_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_start')]),
transitions={'success':'WAIT_VISUAL_SERVOING'})
smach.StateMachine.add('WAIT_VISUAL_SERVOING', gbs.wait_for_events([('/mcr_monitoring/component_wise_pose_error_monitor/event_out','e_stop', True)], timeout_duration=30),
transitions={'success':'STOP_VISUAL_SERVOING',
'timeout': 'STOP_VISUAL_SERVOING_WITH_FAILURE',
'failure':'STOP_VISUAL_SERVOING_WITH_FAILURE'})
smach.StateMachine.add('STOP_VISUAL_SERVOING', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_SUCCESS'})
smach.StateMachine.add('STOP_VISUAL_SERVOING_WITH_FAILURE', gbs.send_event([('/mir_controllers/visual_servoing/event_in','e_stop')]),
transitions={'success':'SET_VISUAL_SERVOING_FAILURE'})
smach.StateMachine.add('SET_VISUAL_SERVOING_SUCCESS', mir_gbs.set_vs_status(status=True),
transitions={'success':'OPEN_GRIPPER_FOR_APPROACH_OBJECT'})
smach.StateMachine.add('SET_VISUAL_SERVOING_FAILURE', mir_gbs.set_vs_status(status=False),
transitions={'success':'SELECT_OBJECT_TO_BE_GRASPED'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_APPROACH_OBJECT', gms.control_gripper('open'),
transitions={'succeeded':'APPROACH_OBJECT'})
smach.StateMachine.add('APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_start')]),
transitions={'success':'WAIT_APPROACH_OBJECT'})
smach.StateMachine.add('WAIT_APPROACH_OBJECT', gbs.wait_for_events([('/guarded_approach_pose/coordinator/event_out','e_success', True)], timeout_duration=6),
transitions={'success':'STOP_APPROACH_OBJECT',
'timeout': 'STOP_APPROACH_OBJECT',
'failure':'STOP_APPROACH_OBJECT'})
smach.StateMachine.add('STOP_APPROACH_OBJECT', gbs.send_event([('/guarded_approach_pose/coordinator/event_in', 'e_stop')]),
transitions={'success':'CLOSE_GRIPPER'})
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM'})
smach.StateMachine.add('GRASP_OBJ', gms.linear_motion(operation='grasp'),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('GRASP_OBJ_VISUAL_SERVOING', gms.linear_motion(operation='grasp', offset_x=-0.05),
transitions={'succeeded':'PRE_PLACE_OBJ_ON_REAR_PLATFORM',
'failed':'DELETE_FROM_RECOGNIZED_OBJECTS'}) # TODO: this could loop
smach.StateMachine.add('PRE_PLACE_OBJ_ON_REAR_PLATFORM', btts.pre_place_obj_on_rear_platform_btt(),
transitions={'succeeded':'VERIFY_GRASPED',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('VERIFY_GRASPED', gbs.send_event([('/gripper_controller/grasp_monitor/event_in', 'e_trigger')]),
transitions={'success':'WAIT_FOR_VERIFY_GRASPED'})
smach.StateMachine.add('WAIT_FOR_VERIFY_GRASPED', gbs.wait_for_events([('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)], timeout_duration=5),
transitions={'success':'PLACE_OBJ_ON_REAR_PLATFORM',
'timeout':'OPEN_GRIPPER_FOR_GRASP_FAILURE',
'failure':'OPEN_GRIPPER_FOR_GRASP_FAILURE'})
smach.StateMachine.add('OPEN_GRIPPER_FOR_GRASP_FAILURE', gms.control_gripper('open'),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS'})
# THESE ARE NEVER ENTERED?
# It would mean we've grasped the object - and don't have somewhere to place it.
# If we don't have somewhere to place it, we won't grasp in the first place?
smach.StateMachine.add('PLACE_OBJ_ON_REAR_PLATFORM', btts.place_obj_on_rear_platform_btt(),
transitions={'succeeded':'DELETE_FROM_RECOGNIZED_OBJECTS',
'no_more_free_poses':'no_more_free_poses'})
smach.StateMachine.add('DELETE_FROM_RECOGNIZED_OBJECTS', btts.delete_from_recognized_objects(),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED'},
remapping={'object_to_delete': 'object_to_grasp'})
smach.StateMachine.add('ADJUST_POSE_WRT_WORKSPACE_NEXT', gns.adjust_to_workspace(0.1),
transitions={'succeeded':'SELECT_OBJECT_TO_BE_GRASPED',
'failed':'ADJUST_POSE_WRT_WORKSPACE_NEXT'})
smach.StateMachine.add('NO_MORE_FREE_POSES_SAFE',gms.move_arm('look_at_workspace'),
transitions={'succeeded':'no_more_free_poses',
'failed':'SKIP_SOURCE_POSE_SAFE'})
# MISC STATES
smach.StateMachine.add('SKIP_SOURCE_POSE_SAFE', gms.move_arm('look_at_workspace'),
transitions={'succeeded':'SKIP_SOURCE_POSE',
'failed':'SKIP_SOURCE_POSE_SAFE'})
smach.StateMachine.add('SKIP_SOURCE_POSE', btts.skip_pose('source'),
transitions={'pose_skipped':'SELECT_SOURCE_SUBTASK',
'pose_skipped_but_platform_limit_reached':'pose_skipped_but_platform_limit_reached'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'failed', 'no_object_for_ppt_platform'],
input_keys=[
'all_found_holes', 'base_pose_to_approach', 'last_grasped_obj',
'move_arm_to', 'move_base_by', 'object_pose',
'rear_platform_free_poses', 'rear_platform_occupied_poses',
'selected_hole', 'selected_hole_pose', 'task_list'
],
output_keys=[
'all_found_holes', 'base_pose_to_approach', 'last_grasped_obj',
'move_arm_to', 'move_base_by', 'rear_platform_free_poses',
'rear_platform_occupied_poses', 'selected_hole',
'selected_hole_pose', 'task_list'
])
with self:
smach.StateMachine.add(
'ADD_WALLS_TO_PLANNING_SCENE',
gms.configure_planning_scene("walls", "add"),
transitions={'succeeded': 'MOVE_ARM_OUT_OF_VIEW'})
smach.StateMachine.add('MOVE_ARM_OUT_OF_VIEW',
gms.move_arm('out_of_view'),
transitions={
'succeeded': 'FIND_HOLES',
'failed': 'MOVE_ARM_OUT_OF_VIEW'
})
smach.StateMachine.add(
'FIND_HOLES',
gps.find_holes(),
transitions={
'found_holes': 'SELECT_HOLE_TO_ADJUST_TO',
'found_no_holes': 'failed', # perform a BTT placement
'timeout': 'failed'
})
smach.StateMachine.add(
'SELECT_HOLE_TO_ADJUST_TO',
ppts.select_hole(),
transitions={
'hole_selected': 'COMPUTE_BASE_SHIFT_TO_OBJECT',
'no_match': 'failed', # perform a BTT placement
'no_more_obj_for_this_workspace':
'no_object_for_ppt_platform'
})
smach.StateMachine.add(
'COMPUTE_BASE_SHIFT_TO_OBJECT',
btts.compute_base_shift_to_object(),
transitions={
'succeeded': 'MOVE_BASE_RELATIVE',
'tf_error': 'COMPUTE_BASE_SHIFT_TO_OBJECT'
},
remapping={'object_pose': 'selected_hole'}
) # TODO: can we do such a remapping, if not, we need a separate "selected_hole_pose" item in the userdata
smach.StateMachine.add('MOVE_BASE_RELATIVE',
gns.move_base_relative(),
transitions={
'succeeded':
'GRASP_OBJECT_FOR_HOLE_FROM_PLTF',
'timeout': 'MOVE_BASE_RELATIVE'
})
smach.StateMachine.add('GRASP_OBJECT_FOR_HOLE_FROM_PLTF',
ppts.grasp_obj_for_hole_from_pltf(),
transitions={
'object_grasped':
'MOVE_TO_INTERMEDIATE_POSE',
'no_more_obj_for_this_workspace':
'no_object_for_ppt_platform'
})
###### DO WE NEED THIS !?!?! BECAUSE OTHERWISE IT WILL HIT THE CAMERA AND ALIGN THE OBJECT "PROPERLY" !?!??!
smach.StateMachine.add('MOVE_TO_INTERMEDIATE_POSE',
gms.move_arm('platform_intermediate'),
transitions={
'succeeded': 'SELECT_ARM_POSITION',
'failed': 'MOVE_TO_INTERMEDIATE_POSE'
})
smach.StateMachine.add(
'SELECT_ARM_POSITION',
ppts.select_arm_position(),
transitions={'arm_pose_selected': 'MOVE_ARM'})
smach.StateMachine.add('MOVE_ARM',
gms.move_arm(),
transitions={
'succeeded': 'MOVE_GRIPPER',
'failed': 'MOVE_ARM'
})
smach.StateMachine.add(
'MOVE_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'MOVE_ARM_TO_INTERMEDIATE_2'})
smach.StateMachine.add('MOVE_ARM_TO_INTERMEDIATE_2',
gms.move_arm('platform_intermediate'),
transitions={
'succeeded': 'succeeded',
'failed': 'MOVE_ARM_TO_INTERMEDIATE_2'
})
def main():
# Open the container
rospy.init_node('insert_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['goal'],
output_keys = ['feedback', 'result'])
with sm:
smach.StateMachine.add('SELECT_OBJECT', SelectObject('/mcr_perception/object_selector/input/object_name'),
transitions={'succeeded':'GENERATE_OBJECT_POSE'})
# generates a pose of object
smach.StateMachine.add('GENERATE_OBJECT_POSE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/object_selector/event_out','e_selected', True)],
timeout_duration=10),
transitions={'success':'SET_DBC_PARAMS',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_try')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'UNSTAGE_OBJECT',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(),
transitions={'success': 'RE_GENERATE_OBJECT_POSE',
'failed' : 'OVERALL_FAILED'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('RE_GENERATE_OBJECT_POSE', gbs.send_event(
[('/mcr_perception/object_selector/event_in', 'e_re_trigger')]),
transitions={'success':'MOVE_ROBOT_AND_PLACE'})
# execute robot motion
smach.StateMachine.add('MOVE_ROBOT_AND_PLACE', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=20),
transitions={'success':'OPEN_GRIPPER',
'timeout':'MOVE_ARM_TO_DEFAULT_PLACE',
'failure':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'MOVE_ARM_CARTESIAN',
'failed':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('MOVE_ARM_CARTESIAN', gbs.send_and_wait_events_combined(
event_in_list=[('/insert_object_workaroud/event_in','e_start')],
event_out_list=[('/insert_object_workaroud/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'OPEN_GRIPPER',
'timeout':'STOP_ARM_CARTESIAN_MOTION',
'failure':'OPEN_GRIPPER'})
smach.StateMachine.add('STOP_ARM_CARTESIAN_MOTION', gbs.send_event(
[('/insert_object_workaroud/event_in', 'e_stop')]),
transitions={'success':'OPEN_GRIPPER'})
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_object_server',
action_spec = GenericExecuteAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('insert_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['insert_object_goal'],
output_keys = ['insert_object_feedback', 'insert_object_result'])
with sm:
if not mokeup:
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'PLAN_WHOLE_BODY_MOTION',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_WHOLE_BODY_MOTION', wait_for_event('/whole_body_motion_calculator_pipeline/event_out', 15.0),
transitions={'success':'CALCULATE_BASE_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure':'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion
smach.StateMachine.add('CALCULATE_BASE_MOTION', gbs.send_and_wait_events_combined(
event_in_list=[('/base_motion_calculator/event_in','e_start')],
event_out_list=[('/base_motion_calculator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout':'STOP_MOVE_BASE_TO_OBJECT',
'failure':'STOP_MOVE_BASE_TO_OBJECT'})
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'UNSTAGE_OBJECT'})
smach.StateMachine.add('UNSTAGE_OBJECT', unstage(),
transitions={'succeeded': 'MOVE_ROBOT_TO_OBJECT',
'failed' : 'SET_ACTION_LIB_FAILURE'})
# execute robot motion
smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')],
event_out_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_MOVE_BASE_TO_OBJECT',
'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_BASE_TO_OBJECT',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
########################################## PREGRASP PIPELINE 2 ##################################################
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add('SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/pregrasp_planner_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2', wait_for_event('/pregrasp_planner_pipeline/event_out', 15.0),
transitions={'success':'STOP_PLAN_ARM_MOTION_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event('/pregrasp_planner_pipeline/event_in','e_stop'),
transitions={'success':'MOVE_ARM_TO_OBJECT_2'})
# execute robot motion
smach.StateMachine.add('MOVE_ARM_TO_OBJECT_2', gbs.send_and_wait_events_combined(
event_in_list=[('/waypoint_trajectory_generation/event_in','e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'OPEN_GRIPPER'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'MOVE_ARM_TO_DEFAULT_PLACE'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})
# place in case of failure
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("10cm/pose4"),
transitions={'succeeded':'OPEN_GRIPPER_DEFAULT',
'failed':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('OPEN_GRIPPER_DEFAULT', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_BACK'})
smach.StateMachine.add('MOVE_ARM_BACK', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'SET_ACTION_LIB_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_FAILED',
'failed':'OVERALL_FAILED'})"""
# smach viewer
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_object_server',
action_spec = InsertObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'insert_object_goal',
feedback_key = 'insert_object_feedback',
result_key = 'insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("calibrate_pick_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"STOP_REFERENCE_FRAME_PUB",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_stop")]),
transitions={"success": "OPEN_GRIPPER"},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ARM"},
)
smach.StateMachine.add('MOVE_ARM',
gms.move_arm('look_at_workspace_from_near'),
transitions={
'succeeded': 'PUBLISH_REFERENCE_FRAME',
'failed': 'MOVE_ARM'
})
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"PUBLISH_REFERENCE_FRAME",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_start")]),
transitions={"success": "WAIT_FOR_EVENT_FROM_USER"},
)
smach.StateMachine.add('WAIT_FOR_EVENT_FROM_USER',
WaitForEventFromUser(),
transitions={
'success': 'MOVE_ROBOT_AND_PICK',
'failure': 'OVERALL_FAILED'
})
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
("/waypoint_trajectory_generation/event_in", "e_start"),
("/wbc/event_in", "e_stop"),
]),
transitions={"success": "OVERALL_FAILED"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("calibrate_pick_smach_viewer", sm,
"/CALIBRATE_PICK_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="calibrate_pick_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['pick_object_wbc_goal'],
output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'SET_DBC_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_TO_OBJECT',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=25),
transitions={'success':'STOP_MOVE_ROBOT_TO_OBJECT',
'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'})
# execute robot motion
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]),
transitions={'success':'CLOSE_GRIPPER'})
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]),
transitions={'success':'SET_ACTION_LIB_FAILURE'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'VERIFY_OBJECT_GRASPED',
'failed':'MOVE_ARM_TO_HOLD'})
smach.StateMachine.add('VERIFY_OBJECT_GRASPED', gbs.send_and_wait_events_combined(
event_in_list=[('/gripper_controller/grasp_monitor/event_in','e_trigger')],
event_out_list=[('/gripper_controller/grasp_monitor/event_out','e_object_grasped', True)],
timeout_duration=5),
transitions={'success':'SET_ACTION_LIB_SUCCESS',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'wbc_pick_object_server',
action_spec = PickObjectWBCAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'pick_object_wbc_goal',
feedback_key = 'pick_object_wbc_feedback',
result_key = 'pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('stage_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['stage_object_goal'],
output_keys = ['stage_object_feedback', 'stage_object_result'])
with sm:
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE', SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE',
'failed': 'SETUP_MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE', gms.move_arm(),
transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE', SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE',
'failed': 'SETUP_MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE', gms.move_arm(),
transitions={'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE_AGAIN', SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN',
'failed': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN', gms.move_arm(),
transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# smach viewer
sis = smach_ros.IntrospectionServer('stage_object_smach_viewer', sm, '/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'stage_object_server',
action_spec = StageObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'stage_object_goal',
feedback_key = 'stage_object_feedback',
result_key = 'stage_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("insert_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in",
"e_trigger")],
event_out_list=[(
"/mcr_perception/object_selector/event_out",
"e_selected",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "RE_GENERATE_OBJECT_POSE",
"failed": "OVERALL_FAILED",
},
)
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
"RE_GENERATE_OBJECT_POSE",
gbs.send_event([("/mcr_perception/object_selector/event_in",
"e_re_trigger")]),
transitions={"success": "MOVE_ROBOT_AND_PLACE"},
)
# execute robot motion
smach.StateMachine.add(
"MOVE_ROBOT_AND_PLACE",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "MOVE_ARM_TO_DEFAULT_PLACE",
"failure": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "MOVE_ARM_CARTESIAN",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_CARTESIAN",
gbs.send_and_wait_events_combined(
event_in_list=[("/insert_object_workaroud/event_in", "e_start")
],
event_out_list=[("/insert_object_workaroud/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STOP_ARM_CARTESIAN_MOTION",
"failure": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"STOP_ARM_CARTESIAN_MOTION",
gbs.send_event([("/insert_object_workaroud/event_in", "e_stop")]),
transitions={"success": "OPEN_GRIPPER"},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "MOVE_ARM_TO_HOLD"},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_HOLD",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("insert_object_smach_viewer", sm,
"/INSERT_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("unstage_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
# read heavy object list
sm.userdata.heavy_objects = rospy.get_param("~heavy_objects", ["m20_100"])
with sm:
# add states to the container
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "CHECK_IF_OBJECT_HEAVY",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_HEAVY",
IsObjectHeavy(),
transitions={
"heavy": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
"light": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE",
"failed": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE",
"failed": "MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE",
SetupMoveArm("final"),
transitions={
"succeeded": "MOVE_ARM_STAGE",
"failed": "SETUP_MOVE_ARM_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE",
gms.move_arm(),
transitions={
"succeeded": "CLOSE_GRIPPER",
"failed": "MOVE_ARM_STAGE"
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
SetupMoveArm("pre", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_HEAVY",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_HEAVY",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE_HEAVY",
"failed": "MOVE_ARM_PRE_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE_HEAVY",
SetupMoveArm("final", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_STAGE_HEAVY",
"failed": "SETUP_MOVE_ARM_STAGE_HEAVY",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE_HEAVY",
gms.move_arm(),
transitions={
"succeeded": "CLOSE_GRIPPER",
"failed": "MOVE_ARM_STAGE_HEAVY"
},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "CHECK_IF_OBJECT_HEAVY_AGAIN"},
)
# TODO: verify if object is grasped or not
smach.StateMachine.add(
"CHECK_IF_OBJECT_HEAVY_AGAIN",
IsObjectHeavy(),
transitions={
"heavy": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
"light": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_AGAIN",
gms.move_arm(blocking=True),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
SetupMoveArm("pre", is_heavy=True),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
gms.move_arm(blocking=False),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_HEAVY_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("unstage_object_smach_viewer", sm,
"/UNSTAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="unstage_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['pick_object_wbc_goal'],
output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'COLLISION_VELOCITY_CHECKER',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('COLLISION_VELOCITY_CHECKER', gbs.send_and_wait_events_combined(
event_in_list=[("/base_collision_checker_pipeline/pose_transformer/event_in",'e_start'),
('/base_collision_checker_pipeline/base_collision_checker_node/event_in','e_start')],
event_out_list=[('/base_collision_checker_pipeline/pose_transformer/event_out','e_success', True),
('/base_collision_checker_pipeline/base_collision_checker_node/event_out','e_success', True)],
timeout_duration=30),
transitions={'success':'STOP_COLLISION_CHECKER',
'timeout':'STOP_COLLISION_CHECKER_FAILURE',
'failure':'STOP_COLLISION_CHECKER_FAILURE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_COLLISION_CHECKER', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'),
transitions={'success':'STOP_POSE_TRANSFORMER'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_POSE_TRANSFORMER', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'),
transitions={'success':'SET_ACTION_LIB_SUCCESS'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_COLLISION_CHECKER_FAILURE', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'),
transitions={'success':'STOP_POSE_TRANSFORMER_FAILURE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_POSE_TRANSFORMER_FAILURE', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'),
transitions={'success':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'wbc_pick_object_server',
action_spec = PickObjectWBCAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'pick_object_wbc_goal',
feedback_key = 'pick_object_wbc_feedback',
result_key = 'pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("insert_cavity_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "STOP_POSE_SELECTOR",
"timeout": "STOP_POSE_SELECTOR",
"failure": "STOP_POSE_SELECTOR",
},
)
smach.StateMachine.add(
"STOP_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "PERCEIVE_CAVITY"},
)
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add(
"PERCEIVE_CAVITY",
gas.perceive_cavity(),
transitions={
"success": "SELECT_OBJECT_AGAIN",
"failed": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_AGAIN",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "TRY_INSERTING",
"failed": "OVERALL_FAILED",
},
)
# execute robot motion
smach.StateMachine.add(
"TRY_INSERTING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start_arm_only")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STAGE_OBJECT",
"failure": "STAGE_OBJECT",
},
)
smach.StateMachine.add(
"STAGE_OBJECT",
gas.stage_object(),
transitions={
"success": "OVERALL_FAILED",
"failed": "OVERALL_FAILED",
},
)
# close gripper
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "WIGGLE_ARM"},
)
# wiggling the arm for precision placement
smach.StateMachine.add(
"WIGGLE_ARM",
ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={
"succeeded": "MOVE_ARM_TO_HOLD",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "STOP_POSE_SELECTOR_FINAL",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# sending e_stop to pose selector
smach.StateMachine.add(
"STOP_POSE_SELECTOR_FINAL",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "OVERALL_SUCCESS"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("insert_cavity_smach_viewer", sm,
"/INSERT_CAVITY_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_cavity_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['unstage_object_goal'],
output_keys=['unstage_object_feedback', 'unstage_object_result'])
with sm:
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open_release'),
transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE'})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
# event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
# event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
# timeout_duration=5),
# transitions={'success':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# smach viewer
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=UnStageObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='unstage_object_goal',
feedback_key='unstage_object_feedback',
result_key='unstage_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['pick_object_goal'],
output_keys=['pick_object_feedback', 'pick_object_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object(
'/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
send_event('/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
wait_for_event('/mcr_perception/object_selector/event_out',
1.0),
transitions={
'success': 'RECONFIGURE_PREGRASP_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure pregrasp planner for pick
smach.StateMachine.add(
'RECONFIGURE_PREGRASP_PARAMS',
gbs.set_named_config('pick_normal_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'RECONFIGURE_PREGRASP_PARAMS'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose and wait for the result of the pregrasp planner
smach.StateMachine.add(
'PLAN_ARM_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[('/pregrasp_planner_pipeline/event_in',
'e_start')],
event_out_list=[('/pregrasp_planner_pipeline/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_ARM_MOTION',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose and wait for its response
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/move_arm_planned/event_in', 'e_start')],
event_out_list=[('/move_arm_planned/event_out', 'e_success',
True)],
timeout_duration=7),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT',
send_event('/move_arm_planned/event_in',
'e_stop'),
transitions={'success': 'CLOSE_GRIPPER'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# check if gripper changed in position, if not then close again
# smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined(
# event_in_list=[('/gripper_state_monitor/event_in','e_trigger')],
# event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)],
# timeout_duration=5),
# transitions={'success':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# optocoupler check if robot has a object in the gripper, if not then pick failed
# smach.StateMachine.add('VERIFY_OBJECT_GRASPED_OPTOCOUPLER', gbs.send_and_wait_events_combined(
# event_in_list=[('/optocoupler_gripper_status/event_in','e_trigger')],
# event_out_list=[('/optocoupler_gripper_status/event_out','e_grasped', True)],
# timeout_duration=5),
# transitions={'success':'SET_ACTION_LIB_SUCCESS',
# 'timeout':'SET_ACTION_LIB_FAILURE',
# 'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='pick_object_server',
action_spec=PickObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='pick_object_goal',
feedback_key='pick_object_feedback',
result_key='pick_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("stage_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "SETUP_MOVE_ARM_PRE_STAGE",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE",
"failed": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE",
"failed": "MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE",
SetupMoveArm("final"),
transitions={
"succeeded": "MOVE_ARM_STAGE",
"failed": "SETUP_MOVE_ARM_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_STAGE"
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN"},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE_AGAIN",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_AGAIN",
gms.move_arm(blocking=False),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("stage_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="stage_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2'
})
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm_and_gripper('open_narrow'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
#TODO: verify if object is grasped or not
smach.StateMachine.add(
'SETUP_MOVE_ARM_PRE_STAGE_AGAIN',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN',
gms.move_arm(),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
