def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(self,
outcomes=['success', 'failure'],
input_keys=[
'move_arm_to', 'reference_point',
'end_effector_pose', 'benchmark_state',
'logging_status'
])
with self:
smach.StateMachine.add('MOVE_ARM_TO_CALIBRATION_CONFIGURATION',
gms.move_arm(),
transitions={
'succeeded': 'START_LOGGING',
'failed': 'START_LOGGING'
})
smach.StateMachine.add(
'START_LOGGING',
start_logging_offline_data(log_benchmark_data),
transitions={'success': 'STOP_COMPUTE_TASK_FRAME_TRANSFORM'})
smach.StateMachine.add(
'STOP_COMPUTE_TASK_FRAME_TRANSFORM',
gbs.send_and_wait_events_combined(
event_in_list=[('/compute_transform/event_in', 'e_stop')],
event_out_list=[('/compute_transform/event_out',
'e_stopped', True)],
timeout_duration=15),
transitions={
'success': 'PUBLISH_REFERENCE_POINT',
'timeout': 'PUBLISH_REFERENCE_POINT',
'failure': 'PUBLISH_REFERENCE_POINT'
})
smach.StateMachine.add(
'PUBLISH_REFERENCE_POINT',
cfs.publish_task_data(
topic_name='/compute_transform/reference_point'),
transitions={
'success': 'COMPUTE_TASK_FRAME_TRANSFORM',
'failure': 'failure'
},
remapping={'task_data': 'reference_point'})
smach.StateMachine.add(
'COMPUTE_TASK_FRAME_TRANSFORM',
gbs.send_and_wait_events_combined(
event_in_list=[('/compute_transform/event_in', 'e_start')],
event_out_list=[('/compute_transform/event_out',
'e_success', True)],
timeout_duration=15),
transitions={
'success': 'success',
'timeout': 'failure',
'failure': 'failure'
})
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(self,
outcomes=['success', 'failure'],
input_keys=['logging_status'])
with self:
smach.StateMachine.add(
'STOP_LOGGING',
stop_logging_offline_data(log_benchmark_data),
transitions={'success': 'STOP_ALL'})
smach.StateMachine.add(
'STOP_ALL',
gbs.send_and_wait_events_combined(event_in_list=[
('/compute_transform/event_in',
'e_stop'), ('/linear_interpolator_demo/event_in',
'e_stop'),
('/linear_interpolator_demo_trajectory_executor/event_in',
'e_stop'), ('/end_effector_pose_logger/event_in',
'e_stop')
]),
transitions={
'success': 'success',
'timeout': 'failure',
'failure': 'failure'
})
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(self,
outcomes=['success'],
input_keys=['logging_status'])
with self:
if log_benchmark_data:
smach.StateMachine.add(
'RESET_FBM',
cfs.reset_fbm(),
transitions={'success': 'STOP_LOGGING_OFFLINE_DATA'})
smach.StateMachine.add(
'STOP_LOGGING_OFFLINE_DATA',
gbs.send_and_wait_events_combined(
event_in_list=[('/rosbag_recorder/event_in',
'e_stop')]),
transitions={
'success': 'SEND_LOGGING_STATUS',
'timeout': 'SEND_LOGGING_STATUS',
'failure': 'SEND_LOGGING_STATUS'
})
else:
smach.StateMachine.add(
'RESET_FBM',
cfs.reset_fbm(),
transitions={'success': 'SEND_LOGGING_STATUS'})
smach.StateMachine.add('SEND_LOGGING_STATUS',
rockin_refbox.send_refbox_logging_status(),
transitions={'done': 'success'})
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(
self, outcomes=["success"], input_keys=["logging_status"]
)
with self:
if log_benchmark_data:
smach.StateMachine.add(
"RESET_FBM",
cfs.reset_fbm(),
transitions={"success": "STOP_LOGGING_OFFLINE_DATA"},
)
smach.StateMachine.add(
"STOP_LOGGING_OFFLINE_DATA",
gbs.send_and_wait_events_combined(
event_in_list=[("/rosbag_recorder/event_in", "e_stop")]
),
transitions={
"success": "SEND_LOGGING_STATUS",
"timeout": "SEND_LOGGING_STATUS",
"failure": "SEND_LOGGING_STATUS",
},
)
else:
smach.StateMachine.add(
"RESET_FBM",
cfs.reset_fbm(),
transitions={"success": "SEND_LOGGING_STATUS"},
)
smach.StateMachine.add(
"SEND_LOGGING_STATUS",
rockin_refbox.send_refbox_logging_status(),
transitions={"done": "success"},
)
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(
self, outcomes=["success", "failure"], input_keys=["logging_status"]
)
with self:
smach.StateMachine.add(
"STOP_LOGGING",
stop_logging_offline_data(log_benchmark_data),
transitions={"success": "STOP_ALL"},
)
smach.StateMachine.add(
"STOP_ALL",
gbs.send_and_wait_events_combined(
event_in_list=[
("/compute_transform/event_in", "e_stop"),
("/linear_interpolator_demo/event_in", "e_stop"),
(
"/linear_interpolator_demo_trajectory_executor/event_in",
"e_stop",
),
("/end_effector_pose_logger/event_in", "e_stop"),
]
),
transitions={
"success": "success",
"timeout": "failure",
"failure": "failure",
},
)
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['success', 'failure'])
with self:
smach.StateMachine.add(
'STOP_ALL',
gbs.send_and_wait_events_combined(event_in_list=[
('/compute_transform/event_in',
'e_stop'), ('/linear_interpolator_demo/event_in',
'e_stop'),
('/linear_interpolator_demo_trajectory_executor/event_in',
'e_stop'), ('/end_effector_pose_logger/event_in',
'e_stop')
]),
transitions={
'success': 'success',
'timeout': 'failure',
'failure': 'failure'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=["success", "failure"],
input_keys=[
"reference_point",
"start_point",
"goal_point",
"move_arm_to",
"logging_status",
],
output_keys=[
"reference_point",
"start_point",
"goal_point",
"move_arm_to",
"logging_status",
],
)
with self:
smach.StateMachine.add(
"STOP_ALL",
stop_all(),
transitions={
"success": "START_END_EFFECTOR_POSE_LOGGER",
"failure": "failure",
},
)
smach.StateMachine.add(
"START_END_EFFECTOR_POSE_LOGGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/end_effector_pose_logger/event_in", "e_start")]
),
transitions={
"success": "INIT_DATA",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"INIT_DATA", cfs.initialize_fbm(), transitions={"success": "success"}
)
def __init__(self):
smach.StateMachine.__init__(self, outcomes=["success", "failure"])
with self:
smach.StateMachine.add(
"STOP_ALL",
gbs.send_and_wait_events_combined(
event_in_list=[
("/compute_transform/event_in", "e_stop"),
("/linear_interpolator_demo/event_in", "e_stop"),
(
"/linear_interpolator_demo_trajectory_executor/event_in",
"e_stop",
),
("/end_effector_pose_logger/event_in", "e_stop"),
]
),
transitions={
"success": "success",
"timeout": "failure",
"failure": "failure",
},
)
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['success', 'failure'],
input_keys=[
'reference_point', 'start_point',
'goal_point', 'move_arm_to',
'logging_status'
],
output_keys=[
'reference_point', 'start_point',
'goal_point', 'move_arm_to',
'logging_status'
])
with self:
smach.StateMachine.add('STOP_ALL',
stop_all(),
transitions={
'success':
'START_END_EFFECTOR_POSE_LOGGER',
'failure': 'failure'
})
smach.StateMachine.add(
'START_END_EFFECTOR_POSE_LOGGER',
gbs.send_and_wait_events_combined(
event_in_list=[('/end_effector_pose_logger/event_in',
'e_start')]),
transitions={
'success': 'INIT_DATA',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('INIT_DATA',
cfs.initialize_fbm(),
transitions={'success': 'success'})
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'],
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 __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):
smach.StateMachine.__init__(self,
outcomes=['success', 'failure'],
input_keys=['start', 'goal'])
with self:
smach.StateMachine.add(
'PUBLISH_START_POINT',
cfs.publish_task_data(
topic_name='/path_generator/start_point'),
transitions={
'success': 'PUBLISH_GOAL_POINT',
'failure': 'failure'
},
remapping={'task_data': 'start'})
smach.StateMachine.add(
'PUBLISH_GOAL_POINT',
cfs.publish_task_data(topic_name='/path_generator/end_point'),
transitions={
'success': 'PLAN_PATH',
'failure': 'failure'
},
remapping={'task_data': 'goal'})
smach.StateMachine.add(
'PLAN_PATH',
gbs.send_and_wait_events_combined(
event_in_list=[('/linear_interpolator_demo/event_in',
'e_start')],
event_out_list=[('/linear_interpolator_demo/event_out',
'e_success', True)],
timeout_duration=30),
transitions={
'success': 'EXECUTE_PLAN',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'EXECUTE_PLAN',
gbs.send_and_wait_events_combined(
event_in_list=
[('/linear_interpolator_demo_trajectory_executor/event_in',
'e_start')],
event_out_list=
[('/linear_interpolator_demo_trajectory_executor/event_out',
'e_success', True)],
timeout_duration=60),
transitions={
'success': 'STOP_ALL',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'STOP_ALL',
gbs.send_and_wait_events_combined(event_in_list=[
('/linear_interpolator_demo/event_in', 'e_stop'),
('/linear_interpolator_demo_trajectory_executor/event_in',
'e_stop')
],
timeout_duration=15),
transitions={
'success': 'success',
'timeout': 'failure',
'failure': 'failure'
})
def __init__(self, log_benchmark_data=False):
smach.StateMachine.__init__(
self,
outcomes=["success", "failure"],
input_keys=[
"move_arm_to",
"reference_point",
"end_effector_pose",
"benchmark_state",
"logging_status",
],
)
with self:
smach.StateMachine.add(
"MOVE_ARM_TO_CALIBRATION_CONFIGURATION",
gms.move_arm(),
transitions={"succeeded": "START_LOGGING", "failed": "START_LOGGING"},
)
smach.StateMachine.add(
"START_LOGGING",
start_logging_offline_data(log_benchmark_data),
transitions={"success": "STOP_COMPUTE_TASK_FRAME_TRANSFORM"},
)
smach.StateMachine.add(
"STOP_COMPUTE_TASK_FRAME_TRANSFORM",
gbs.send_and_wait_events_combined(
event_in_list=[("/compute_transform/event_in", "e_stop")],
event_out_list=[
("/compute_transform/event_out", "e_stopped", True)
],
timeout_duration=15,
),
transitions={
"success": "PUBLISH_REFERENCE_POINT",
"timeout": "PUBLISH_REFERENCE_POINT",
"failure": "PUBLISH_REFERENCE_POINT",
},
)
smach.StateMachine.add(
"PUBLISH_REFERENCE_POINT",
cfs.publish_task_data(topic_name="/compute_transform/reference_point"),
transitions={
"success": "COMPUTE_TASK_FRAME_TRANSFORM",
"failure": "failure",
},
remapping={"task_data": "reference_point"},
)
smach.StateMachine.add(
"COMPUTE_TASK_FRAME_TRANSFORM",
gbs.send_and_wait_events_combined(
event_in_list=[("/compute_transform/event_in", "e_start")],
event_out_list=[
("/compute_transform/event_out", "e_success", True)
],
timeout_duration=15,
),
transitions={
"success": "success",
"timeout": "failure",
"failure": "failure",
},
)
def __init__(self):
smach.StateMachine.__init__(
self, outcomes=["success", "failure"], input_keys=["start", "goal"]
)
with self:
smach.StateMachine.add(
"PUBLISH_START_POINT",
cfs.publish_task_data(topic_name="/path_generator/start_point"),
transitions={"success": "PUBLISH_GOAL_POINT", "failure": "failure"},
remapping={"task_data": "start"},
)
smach.StateMachine.add(
"PUBLISH_GOAL_POINT",
cfs.publish_task_data(topic_name="/path_generator/end_point"),
transitions={"success": "PLAN_PATH", "failure": "failure"},
remapping={"task_data": "goal"},
)
smach.StateMachine.add(
"PLAN_PATH",
gbs.send_and_wait_events_combined(
event_in_list=[("/linear_interpolator_demo/event_in", "e_start")],
event_out_list=[
("/linear_interpolator_demo/event_out", "e_success", True)
],
timeout_duration=30,
),
transitions={
"success": "EXECUTE_PLAN",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"EXECUTE_PLAN",
gbs.send_and_wait_events_combined(
event_in_list=[
(
"/linear_interpolator_demo_trajectory_executor/event_in",
"e_start",
)
],
event_out_list=[
(
"/linear_interpolator_demo_trajectory_executor/event_out",
"e_success",
True,
)
],
timeout_duration=60,
),
transitions={
"success": "STOP_ALL",
"timeout": "failure",
"failure": "failure",
},
)
smach.StateMachine.add(
"STOP_ALL",
gbs.send_and_wait_events_combined(
event_in_list=[
("/linear_interpolator_demo/event_in", "e_stop"),
(
"/linear_interpolator_demo_trajectory_executor/event_in",
"e_stop",
),
],
timeout_duration=15,
),
transitions={
"success": "success",
"timeout": "failure",
"failure": "failure",
},
)
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"},
)