def blocking_go_to_prep_pose(robot, dock_id, timeout=20):
"""Blocking helper that takes control of the robot and takes it to the prep pose only.
Args:
robot: The instance of the robot to control.
dock_id: The ID of the dock to use.
Returns:
None
Raises:
CommandFailedError: The robot was unable to go to the prep pose. See error for details.
"""
docking_client = robot.ensure_client(DockingClient.default_service_name)
converter = robot.time_sync.get_robot_time_converter()
start_time = converter.robot_seconds_from_local_seconds(now_sec())
cmd_end_time = start_time + timeout
cmd_timeout = cmd_end_time + 10 # client side buffer
cmd_id = docking_client.docking_command(
dock_id, robot.time_sync.endpoint.clock_identifier,
seconds_to_timestamp(cmd_end_time), docking_pb2.PREP_POSE_ONLY_POSE)
while converter.robot_seconds_from_local_seconds(now_sec()) < cmd_timeout:
feedback = docking_client.docking_command_feedback_full(cmd_id)
maybe_raise(common_lease_errors(feedback))
status = feedback.status
if status == docking_pb2.DockingCommandFeedbackResponse.STATUS_IN_PROGRESS:
# keep waiting/trying
time.sleep(1)
elif status == docking_pb2.DockingCommandFeedbackResponse.STATUS_AT_PREP_POSE:
return
else:
raise CommandFailedError(
"Failed to go to the prep pose, status: '%s'" %
docking_pb2.DockingCommandFeedbackResponse.Status.Name(status))
raise CommandFailedError("Error going to the prep pose, timeout exceeded.")
def blocking_go_to_prep_pose(robot, dock_id, timeout=20):
"""Blocking helper that takes control of the robot and takes it to the prep pose only.
Args:
robot: The instance of the robot to control.
dock_id: The ID of the dock to use.
Returns:
None
Raises:
CommandFailedError: The robot was unable to go to the prep pose. See error for details.
"""
docking_client = robot.ensure_client(DockingClient.default_service_name)
# Try and put the robot in a safe position
cmd_end_time = time.time() + timeout
cmd_timeout = cmd_end_time + 10 # client side buffer
cmd_id = docking_client.docking_command(
dock_id, robot.time_sync.endpoint.clock_identifier,
robot.time_sync.robot_timestamp_from_local_secs(cmd_end_time),
docking_pb2.PREP_POSE_ONLY_POSE)
while time.time() < cmd_timeout:
status = docking_client.docking_command_feedback(cmd_id)
if status == docking_pb2.DockingCommandFeedbackResponse.STATUS_IN_PROGRESS:
# keep waiting/trying
time.sleep(1)
elif status == docking_pb2.DockingCommandFeedbackResponse.STATUS_AT_PREP_POSE:
return
else:
raise CommandFailedError(
"Failed to go to the prep pose, status: '%s'" %
docking_pb2.DockingCommandFeedbackResponse.Status.Name(status))
raise CommandFailedError("Error going to the prep pose, timeout exceeded.")
def blocking_undock(robot, timeout=20):
"""Blocking helper that undocks the robot from the currently docked dock.
Args:
robot: The instance of the robot to control.
Returns:
None
Raises:
CommandFailedError: The robot was unable to undock. See error for details.
"""
docking_client = robot.ensure_client(DockingClient.default_service_name)
# Try and put the robot in a safe position
cmd_end_time = time.time() + timeout
cmd_timeout = cmd_end_time + 10 # client side buffer
cmd_id = docking_client.docking_command(
0, robot.time_sync.endpoint.clock_identifier,
robot.time_sync.robot_timestamp_from_local_secs(cmd_end_time),
docking_pb2.PREP_POSE_UNDOCK)
while time.time() < cmd_timeout:
status = docking_client.docking_command_feedback(cmd_id)
if status == docking_pb2.DockingCommandFeedbackResponse.STATUS_IN_PROGRESS:
# keep waiting/trying
time.sleep(1)
elif status == docking_pb2.DockingCommandFeedbackResponse.STATUS_AT_PREP_POSE:
return
else:
raise CommandFailedError(
"Failed to undock the robot, status: '%s'" %
docking_pb2.DockingCommandFeedbackResponse.Status.Name(status))
raise CommandFailedError("Error undocking the robot, timeout exceeded.")
def blocking_dock_robot(robot, dock_id, num_retries=4):
"""Blocking helper that takes control of the robot and docks it.
Args:
robot: The instance of the robot to control.
dock_id: The ID of the dock to dock at.
num_retries: Optional number of retries.
Returns:
None
Raises:
CommandFailedError: The robot was unable to be docked. See error for details.
"""
docking_client = robot.ensure_client(DockingClient.default_service_name)
retry_number = 0
docking_success = False
# Try to dock the robot
while retry_number < num_retries and not docking_success:
retry_number += 1
cmd_end_time = time.time() + 30 # expect to finish in 30 seconds
cmd_timeout = cmd_end_time + 10 # client side buffer
prep_pose = (docking_pb2.PREP_POSE_USE_POSE if
(retry_number % 2) else docking_pb2.PREP_POSE_SKIP_POSE)
cmd_id = docking_client.docking_command(
dock_id, robot.time_sync.endpoint.clock_identifier,
robot.time_sync.robot_timestamp_from_local_secs(cmd_end_time),
prep_pose)
while time.time() < cmd_timeout:
status = docking_client.docking_command_feedback(cmd_id)
if status == docking_pb2.DockingCommandFeedbackResponse.STATUS_IN_PROGRESS:
# keep waiting/trying
time.sleep(1)
elif status == docking_pb2.DockingCommandFeedbackResponse.STATUS_DOCKED:
docking_success = True
break
elif (status in [
docking_pb2.DockingCommandFeedbackResponse.
STATUS_MISALIGNED,
docking_pb2.DockingCommandFeedbackResponse.
STATUS_ERROR_COMMAND_TIMED_OUT,
]):
# Retry
break
else:
raise CommandFailedError(
"Docking Failed, status: '%s'" %
docking_pb2.DockingCommandFeedbackResponse.Status.Name(
status))
if docking_success:
return
# Try and put the robot in a safe position
try:
blocking_go_to_prep_pose(robot, dock_id)
except CommandFailedError as error:
pass
# Raise error on original failure to dock
raise CommandFailedError("Docking Failed, too many attempts")
def blocking_dock_robot(robot, dock_id, num_retries=4, timeout=30):
"""Blocking helper that takes control of the robot and docks it.
Args:
robot: The instance of the robot to control.
dock_id: The ID of the dock to dock at.
num_retries: Optional, number of attempts.
Returns:
The number of retries required
Raises:
CommandFailedError: The robot was unable to be docked. See error for details.
"""
docking_client = robot.ensure_client(DockingClient.default_service_name)
attempt_number = 0
docking_success = False
# Try to dock the robot
while attempt_number < num_retries and not docking_success:
attempt_number += 1
converter = robot.time_sync.get_robot_time_converter()
start_time = converter.robot_seconds_from_local_seconds(now_sec())
cmd_end_time = start_time + timeout
cmd_timeout = cmd_end_time + 10 # client side buffer
prep_pose = (docking_pb2.PREP_POSE_USE_POSE if
(attempt_number % 2) else docking_pb2.PREP_POSE_SKIP_POSE)
cmd_id = docking_client.docking_command(
dock_id, robot.time_sync.endpoint.clock_identifier,
seconds_to_timestamp(cmd_end_time), prep_pose)
while converter.robot_seconds_from_local_seconds(
now_sec()) < cmd_timeout:
feedback = docking_client.docking_command_feedback_full(cmd_id)
maybe_raise(common_lease_errors(feedback))
status = feedback.status
if status == docking_pb2.DockingCommandFeedbackResponse.STATUS_IN_PROGRESS:
# keep waiting/trying
time.sleep(1)
elif status == docking_pb2.DockingCommandFeedbackResponse.STATUS_DOCKED:
docking_success = True
break
elif (status in [
docking_pb2.DockingCommandFeedbackResponse.
STATUS_MISALIGNED,
docking_pb2.DockingCommandFeedbackResponse.
STATUS_ERROR_COMMAND_TIMED_OUT,
]):
# Retry
break
else:
raise CommandFailedError(
"Docking Failed, status: '%s'" %
docking_pb2.DockingCommandFeedbackResponse.Status.Name(
status))
if docking_success:
return attempt_number - 1
# Try and put the robot in a safe position
try:
blocking_go_to_prep_pose(robot, dock_id)
except CommandFailedError:
pass
# Raise error on original failure to dock
raise CommandFailedError("Docking Failed, too many attempts")