class EstopNoGui():
"""Provides a software estop without a GUI.
To use this estop, create an instance of the EstopNoGui class and use the stop() and allow()
functions programmatically.
"""
def __init__(self, client, timeout_sec, name=None):
# Force server to set up a single endpoint system
ep = EstopEndpoint(client, name, timeout_sec)
ep.force_simple_setup()
# Begin periodic check-in between keep-alive and robot
self.estop_keep_alive = EstopKeepAlive(ep)
# Release the estop
self.estop_keep_alive.allow()
def __enter__(self):
pass
def __exit__(self, exc_type, exc_val, exc_tb):
"""Cleanly shut down estop on exit."""
self.estop_keep_alive.end_periodic_check_in()
def stop(self):
self.estop_keep_alive.stop()
def allow(self):
self.estop_keep_alive.allow()
def settle_then_cut(self):
self.estop_keep_alive.settle_then_cut()
class SpotWrapper():
"""Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically"""
def __init__(self,
username,
password,
hostname,
logger,
rates={},
callbacks={}):
self._username = username
self._password = password
self._hostname = hostname
self._logger = logger
self._rates = rates
self._callbacks = callbacks
self._keep_alive = True
self._valid = True
self._mobility_params = RobotCommandBuilder.mobility_params()
self._is_standing = False
self._is_sitting = True
self._is_moving = False
self._at_goal = False
self._last_stand_command = None
self._last_sit_command = None
self._last_trajectory_command = None
self._last_velocity_command_time = None
self._front_image_requests = []
for source in front_image_sources:
self._front_image_requests.append(
build_image_request(source,
image_format=image_pb2.Image.FORMAT_RAW))
self._side_image_requests = []
for source in side_image_sources:
self._side_image_requests.append(
build_image_request(source,
image_format=image_pb2.Image.FORMAT_RAW))
self._rear_image_requests = []
for source in rear_image_sources:
self._rear_image_requests.append(
build_image_request(source,
image_format=image_pb2.Image.FORMAT_RAW))
try:
self._sdk = create_standard_sdk('ros_spot')
except Exception as e:
self._logger.error("Error creating SDK object: %s", e)
self._valid = False
return
self._robot = self._sdk.create_robot(self._hostname)
try:
self._robot.authenticate(self._username, self._password)
self._robot.start_time_sync()
except RpcError as err:
self._logger.error("Failed to communicate with robot: %s", err)
self._valid = False
return
if self._robot:
# Clients
try:
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._graph_nav_client = self._robot.ensure_client(
GraphNavClient.default_service_name)
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
self._lease_wallet = self._lease_client.lease_wallet
self._image_client = self._robot.ensure_client(
ImageClient.default_service_name)
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
except Exception as e:
self._logger.error("Unable to create client service: %s", e)
self._valid = False
return
# Store the most recent knowledge of the state of the robot based on rpc calls.
self._current_graph = None
self._current_edges = dict(
) #maps to_waypoint to list(from_waypoint)
self._current_waypoint_snapshots = dict(
) # maps id to waypoint snapshot
self._current_edge_snapshots = dict() # maps id to edge snapshot
self._current_annotation_name_to_wp_id = dict()
# Async Tasks
self._async_task_list = []
self._robot_state_task = AsyncRobotState(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("robot_state", 0.0)),
self._callbacks.get("robot_state", lambda: None))
self._robot_metrics_task = AsyncMetrics(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("metrics", 0.0)),
self._callbacks.get("metrics", lambda: None))
self._lease_task = AsyncLease(
self._lease_client, self._logger,
max(0.0, self._rates.get("lease", 0.0)),
self._callbacks.get("lease", lambda: None))
self._front_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("front_image", 0.0)),
self._callbacks.get("front_image", lambda: None),
self._front_image_requests)
self._side_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("side_image", 0.0)),
self._callbacks.get("side_image", lambda: None),
self._side_image_requests)
self._rear_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("rear_image", 0.0)),
self._callbacks.get("rear_image", lambda: None),
self._rear_image_requests)
self._idle_task = AsyncIdle(self._robot_command_client,
self._logger, 10.0, self)
self._estop_endpoint = None
self._async_tasks = AsyncTasks([
self._robot_state_task, self._robot_metrics_task,
self._lease_task, self._front_image_task,
self._side_image_task, self._rear_image_task, self._idle_task
])
self._robot_id = None
self._lease = None
@property
def logger(self):
"""Return logger instance of the SpotWrapper"""
return self._logger
@property
def is_valid(self):
"""Return boolean indicating if the wrapper initialized successfully"""
return self._valid
@property
def id(self):
"""Return robot's ID"""
return self._robot_id
@property
def robot_state(self):
"""Return latest proto from the _robot_state_task"""
return self._robot_state_task.proto
@property
def metrics(self):
"""Return latest proto from the _robot_metrics_task"""
return self._robot_metrics_task.proto
@property
def lease(self):
"""Return latest proto from the _lease_task"""
return self._lease_task.proto
@property
def front_images(self):
"""Return latest proto from the _front_image_task"""
return self._front_image_task.proto
@property
def side_images(self):
"""Return latest proto from the _side_image_task"""
return self._side_image_task.proto
@property
def rear_images(self):
"""Return latest proto from the _rear_image_task"""
return self._rear_image_task.proto
@property
def is_standing(self):
"""Return boolean of standing state"""
return self._is_standing
@property
def is_sitting(self):
"""Return boolean of standing state"""
return self._is_sitting
@property
def is_moving(self):
"""Return boolean of walking state"""
return self._is_moving
@property
def at_goal(self):
return self._at_goal
@property
def time_skew(self):
"""Return the time skew between local and spot time"""
return self._robot.time_sync.endpoint.clock_skew
def robotToLocalTime(self, timestamp):
"""Takes a timestamp and an estimated skew and return seconds and nano seconds in local time
Args:
timestamp: google.protobuf.Timestamp
Returns:
google.protobuf.Timestamp
"""
rtime = Timestamp()
rtime.seconds = timestamp.seconds - self.time_skew.seconds
rtime.nanos = timestamp.nanos - self.time_skew.nanos
if rtime.nanos < 0:
rtime.nanos = rtime.nanos + 1000000000
rtime.seconds = rtime.seconds - 1
# Workaround for timestamps being incomplete
if rtime.seconds < 0:
rtime.seconds = 0
rtime.nanos = 0
return rtime
def claim(self):
"""Get a lease for the robot, a handle on the estop endpoint, and the ID of the robot."""
try:
self._robot_id = self._robot.get_id()
self.getLease()
self.resetEStop()
return True, "Success"
except (ResponseError, RpcError) as err:
self._logger.error("Failed to initialize robot communication: %s",
err)
return False, str(err)
def updateTasks(self):
"""Loop through all periodic tasks and update their data if needed."""
self._async_tasks.update()
def resetEStop(self):
"""Get keepalive for eStop"""
self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0)
self._estop_endpoint.force_simple_setup(
) # Set this endpoint as the robot's sole estop.
self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)
def assertEStop(self, severe=True):
"""Forces the robot into eStop state.
Args:
severe: Default True - If true, will cut motor power immediately. If false, will try to settle the robot on the ground first
"""
try:
if severe:
self._estop_keepalive.stop()
else:
self._estop_keepalive.settle_then_cut()
return True, "Success"
except:
return False, "Error"
def disengageEStop(self):
"""Disengages the E-Stop"""
try:
self._estop_keepalive.allow()
return True, "Success"
except:
return False, "Error"
def releaseEStop(self):
"""Stop eStop keepalive"""
if self._estop_keepalive:
self._estop_keepalive.stop()
self._estop_keepalive = None
self._estop_endpoint = None
def getLease(self):
"""Get a lease for the robot and keep the lease alive automatically."""
self._lease = self._lease_client.acquire()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
def releaseLease(self):
"""Return the lease on the body."""
if self._lease:
self._lease_client.return_lease(self._lease)
self._lease = None
def release(self):
"""Return the lease on the body and the eStop handle."""
try:
self.releaseLease()
self.releaseEStop()
return True, "Success"
except Exception as e:
return False, str(e)
def disconnect(self):
"""Release control of robot as gracefully as posssible."""
if self._robot.time_sync:
self._robot.time_sync.stop()
self.releaseLease()
self.releaseEStop()
def _robot_command(self,
command_proto,
end_time_secs=None,
timesync_endpoint=None):
"""Generic blocking function for sending commands to robots.
Args:
command_proto: robot_command_pb2 object to send to the robot. Usually made with RobotCommandBuilder
end_time_secs: (optional) Time-to-live for the command in seconds
timesync_endpoint: (optional) Time sync endpoint
"""
try:
id = self._robot_command_client.robot_command(
lease=None,
command=command_proto,
end_time_secs=end_time_secs,
timesync_endpoint=timesync_endpoint)
return True, "Success", id
except Exception as e:
return False, str(e), None
def stop(self):
"""Stop the robot's motion."""
response = self._robot_command(RobotCommandBuilder.stop_command())
return response[0], response[1]
def self_right(self):
"""Have the robot self-right itself."""
response = self._robot_command(RobotCommandBuilder.selfright_command())
return response[0], response[1]
def sit(self):
"""Stop the robot's motion and sit down if able."""
response = self._robot_command(
RobotCommandBuilder.synchro_sit_command())
self._last_sit_command = response[2]
return response[0], response[1]
def stand(self, monitor_command=True):
"""If the e-stop is enabled, and the motor power is enabled, stand the robot up."""
response = self._robot_command(
RobotCommandBuilder.synchro_stand_command(
params=self._mobility_params))
if monitor_command:
self._last_stand_command = response[2]
return response[0], response[1]
def safe_power_off(self):
"""Stop the robot's motion and sit if possible. Once sitting, disable motor power."""
response = self._robot_command(
RobotCommandBuilder.safe_power_off_command())
return response[0], response[1]
def clear_behavior_fault(self, id):
"""Clear the behavior fault defined by id."""
try:
rid = self._robot_command_client.clear_behavior_fault(
behavior_fault_id=id, lease=None)
return True, "Success", rid
except Exception as e:
return False, str(e), None
def power_on(self):
"""Enble the motor power if e-stop is enabled."""
try:
power.power_on(self._power_client)
return True, "Success"
except:
return False, "Error"
def set_mobility_params(self, mobility_params):
"""Set Params for mobility and movement
Args:
mobility_params: spot.MobilityParams, params for spot mobility commands.
"""
self._mobility_params = mobility_params
def get_mobility_params(self):
"""Get mobility params
"""
return self._mobility_params
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.125):
"""Send a velocity motion command to the robot.
Args:
v_x: Velocity in the X direction in meters
v_y: Velocity in the Y direction in meters
v_rot: Angular velocity around the Z axis in radians
cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate).
"""
end_time = time.time() + cmd_duration
response = self._robot_command(
RobotCommandBuilder.synchro_velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params),
end_time_secs=end_time,
timesync_endpoint=self._robot.time_sync.endpoint)
self._last_velocity_command_time = end_time
return response[0], response[1]
def trajectory_cmd(self,
goal_x,
goal_y,
goal_heading,
cmd_duration,
frame_name='odom'):
"""Send a trajectory motion command to the robot.
Args:
goal_x: Position X coordinate in meters
goal_y: Position Y coordinate in meters
goal_heading: Pose heading in radians
cmd_duration: Time-to-live for the command in seconds.
frame_name: frame_name to be used to calc the target position. 'odom' or 'vision'
"""
self._at_goal = False
self._logger.info("got command duration of {}".format(cmd_duration))
end_time = time.time() + cmd_duration
if frame_name == 'vision':
vision_tform_body = frame_helpers.get_vision_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
vision_tform_goal = vision_tform_body * body_tform_goal
response = self._robot_command(
RobotCommandBuilder.trajectory_command(
goal_x=vision_tform_goal.x,
goal_y=vision_tform_goal.y,
goal_heading=vision_tform_goal.rot.to_yaw(),
frame_name=frame_helpers.VISION_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
elif frame_name == 'odom':
odom_tform_body = frame_helpers.get_odom_tform_body(
self._robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot)
body_tform_goal = math_helpers.SE3Pose(
x=goal_x,
y=goal_y,
z=0,
rot=math_helpers.Quat.from_yaw(goal_heading))
odom_tform_goal = odom_tform_body * body_tform_goal
response = self._robot_command(
RobotCommandBuilder.trajectory_command(
goal_x=odom_tform_goal.x,
goal_y=odom_tform_goal.y,
goal_heading=odom_tform_goal.rot.to_yaw(),
frame_name=frame_helpers.ODOM_FRAME_NAME,
params=self._mobility_params),
end_time_secs=end_time)
else:
raise ValueError('frame_name must be \'vision\' or \'odom\'')
if response[0]:
self._last_trajectory_command = response[2]
return response[0], response[1]
def list_graph(self, upload_path):
"""List waypoint ids of garph_nav
Args:
upload_path : Path to the root directory of the map.
"""
ids, eds = self._list_graph_waypoint_and_edge_ids()
# skip waypoint_ for v2.2.1, skip waypiont for < v2.2
return [
v for k, v in sorted(
ids.items(),
key=lambda id: int(id[0].replace('waypoint_', '')))
]
def navigate_to(self,
upload_path,
navigate_to,
initial_localization_fiducial=True,
initial_localization_waypoint=None):
""" navigate with graph nav.
Args:
upload_path : Path to the root directory of the map.
navigate_to : Waypont id string for where to goal
initial_localization_fiducial : Tells the initializer whether to use fiducials
initial_localization_waypoint : Waypoint id string of current robot position (optional)
"""
# Filepath for uploading a saved graph's and snapshots too.
if upload_path[-1] == "/":
upload_filepath = upload_path[:-1]
else:
upload_filepath = upload_path
# Boolean indicating the robot's power state.
power_state = self._robot_state_client.get_robot_state().power_state
self._started_powered_on = (
power_state.motor_power_state == power_state.STATE_ON)
self._powered_on = self._started_powered_on
# FIX ME somehow,,,, if the robot is stand, need to sit the robot before starting garph nav
if self.is_standing and not self.is_moving:
self.sit()
# TODO verify estop / claim / power_on
self._clear_graph()
self._upload_graph_and_snapshots(upload_filepath)
if initial_localization_fiducial:
self._set_initial_localization_fiducial()
if initial_localization_waypoint:
self._set_initial_localization_waypoint(
[initial_localization_waypoint])
self._list_graph_waypoint_and_edge_ids()
self._get_localization_state()
resp = self._navigate_to([navigate_to])
return resp
## copy from spot-sdk/python/examples/graph_nav_command_line/graph_nav_command_line.py
def _get_localization_state(self, *args):
"""Get the current localization and state of the robot."""
state = self._graph_nav_client.get_localization_state()
self._logger.info('Got localization: \n%s' % str(state.localization))
odom_tform_body = get_odom_tform_body(
state.robot_kinematics.transforms_snapshot)
self._logger.info('Got robot state in kinematic odometry frame: \n%s' %
str(odom_tform_body))
def _set_initial_localization_fiducial(self, *args):
"""Trigger localization when near a fiducial."""
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an empty instance for initial localization since we are asking it to localize
# based on the nearest fiducial.
localization = nav_pb2.Localization()
self._graph_nav_client.set_localization(
initial_guess_localization=localization,
ko_tform_body=current_odom_tform_body)
def _set_initial_localization_waypoint(self, *args):
"""Trigger localization to a waypoint."""
# Take the first argument as the localization waypoint.
if len(args) < 1:
# If no waypoint id is given as input, then return without initializing.
self._logger.error("No waypoint specified to initialize to.")
return
destination_waypoint = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id, self._logger)
if not destination_waypoint:
# Failed to find the unique waypoint id.
return
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an initial localization to the specified waypoint as the identity.
localization = nav_pb2.Localization()
localization.waypoint_id = destination_waypoint
localization.waypoint_tform_body.rotation.w = 1.0
self._graph_nav_client.set_localization(
initial_guess_localization=localization,
# It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m).
max_distance=0.2,
max_yaw=20.0 * math.pi / 180.0,
fiducial_init=graph_nav_pb2.SetLocalizationRequest.
FIDUCIAL_INIT_NO_FIDUCIAL,
ko_tform_body=current_odom_tform_body)
def _list_graph_waypoint_and_edge_ids(self, *args):
"""List the waypoint ids and edge ids of the graph currently on the robot."""
# Download current graph
graph = self._graph_nav_client.download_graph()
if graph is None:
self._logger.error("Empty graph.")
return
self._current_graph = graph
localization_id = self._graph_nav_client.get_localization_state(
).localization.waypoint_id
# Update and print waypoints and edges
self._current_annotation_name_to_wp_id, self._current_edges = graph_nav_util.update_waypoints_and_edges(
graph, localization_id, self._logger)
return self._current_annotation_name_to_wp_id, self._current_edges
def _upload_graph_and_snapshots(self, upload_filepath):
"""Upload the graph and snapshots to the robot."""
self._logger.info("Loading the graph from disk into local storage...")
with open(upload_filepath + "/graph", "rb") as graph_file:
# Load the graph from disk.
data = graph_file.read()
self._current_graph = map_pb2.Graph()
self._current_graph.ParseFromString(data)
self._logger.info(
"Loaded graph has {} waypoints and {} edges".format(
len(self._current_graph.waypoints),
len(self._current_graph.edges)))
for waypoint in self._current_graph.waypoints:
# Load the waypoint snapshots from disk.
with open(
upload_filepath +
"/waypoint_snapshots/{}".format(waypoint.snapshot_id),
"rb") as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
self._current_waypoint_snapshots[
waypoint_snapshot.id] = waypoint_snapshot
for edge in self._current_graph.edges:
# Load the edge snapshots from disk.
with open(
upload_filepath +
"/edge_snapshots/{}".format(edge.snapshot_id),
"rb") as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
self._current_edge_snapshots[edge_snapshot.id] = edge_snapshot
# Upload the graph to the robot.
self._logger.info("Uploading the graph and snapshots to the robot...")
self._graph_nav_client.upload_graph(lease=self._lease.lease_proto,
graph=self._current_graph)
# Upload the snapshots to the robot.
for waypoint_snapshot in self._current_waypoint_snapshots.values():
self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot)
self._logger.info("Uploaded {}".format(waypoint_snapshot.id))
for edge_snapshot in self._current_edge_snapshots.values():
self._graph_nav_client.upload_edge_snapshot(edge_snapshot)
self._logger.info("Uploaded {}".format(edge_snapshot.id))
# The upload is complete! Check that the robot is localized to the graph,
# and it if is not, prompt the user to localize the robot before attempting
# any navigation commands.
localization_state = self._graph_nav_client.get_localization_state()
if not localization_state.localization.waypoint_id:
# The robot is not localized to the newly uploaded graph.
self._logger.info(
"Upload complete! The robot is currently not localized to the map; please localize", \
"the robot using commands (2) or (3) before attempting a navigation command.")
def _navigate_to(self, *args):
"""Navigate to a specific waypoint."""
# Take the first argument as the destination waypoint.
if len(args) < 1:
# If no waypoint id is given as input, then return without requesting navigation.
self._logger.info(
"No waypoint provided as a destination for navigate to.")
return
self._lease = self._lease_wallet.get_lease()
destination_waypoint = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id, self._logger)
if not destination_waypoint:
# Failed to find the appropriate unique waypoint id for the navigation command.
return
if not self.toggle_power(should_power_on=True):
self._logger.info(
"Failed to power on the robot, and cannot complete navigate to request."
)
return
# Stop the lease keepalive and create a new sublease for graph nav.
self._lease = self._lease_wallet.advance()
sublease = self._lease.create_sublease()
self._lease_keepalive.shutdown()
# Navigate to the destination waypoint.
is_finished = False
nav_to_cmd_id = -1
while not is_finished:
# Issue the navigation command about twice a second such that it is easy to terminate the
# navigation command (with estop or killing the program).
nav_to_cmd_id = self._graph_nav_client.navigate_to(
destination_waypoint, 1.0, leases=[sublease.lease_proto])
time.sleep(
.5) # Sleep for half a second to allow for command execution.
# Poll the robot for feedback to determine if the navigation command is complete. Then sit
# the robot down once it is finished.
is_finished = self._check_success(nav_to_cmd_id)
self._lease = self._lease_wallet.advance()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
# Update the lease and power off the robot if appropriate.
if self._powered_on and not self._started_powered_on:
# Sit the robot down + power off after the navigation command is complete.
self.toggle_power(should_power_on=False)
status = self._graph_nav_client.navigation_feedback(nav_to_cmd_id)
if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL:
return True, "Successfully completed the navigation commands!"
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
return False, "Robot got lost when navigating the route, the robot will now sit down."
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
return False, "Robot got stuck when navigating the route, the robot will now sit down."
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
return False, "Robot is impaired."
else:
return False, "Navigation command is not complete yet."
def _navigate_route(self, *args):
"""Navigate through a specific route of waypoints."""
if len(args) < 1:
# If no waypoint ids are given as input, then return without requesting navigation.
self._logger.error("No waypoints provided for navigate route.")
return
waypoint_ids = args[0]
for i in range(len(waypoint_ids)):
waypoint_ids[i] = graph_nav_util.find_unique_waypoint_id(
waypoint_ids[i], self._current_graph,
self._current_annotation_name_to_wp_id, self._logger)
if not waypoint_ids[i]:
# Failed to find the unique waypoint id.
return
edge_ids_list = []
all_edges_found = True
# Attempt to find edges in the current graph that match the ordered waypoint pairs.
# These are necessary to create a valid route.
for i in range(len(waypoint_ids) - 1):
start_wp = waypoint_ids[i]
end_wp = waypoint_ids[i + 1]
edge_id = self._match_edge(self._current_edges, start_wp, end_wp)
if edge_id is not None:
edge_ids_list.append(edge_id)
else:
all_edges_found = False
self._logger.error(
"Failed to find an edge between waypoints: ", start_wp,
" and ", end_wp)
self._logger.error(
"List the graph's waypoints and edges to ensure pairs of waypoints has an edge."
)
break
self._lease = self._lease_wallet.get_lease()
if all_edges_found:
if not self.toggle_power(should_power_on=True):
self._logger.error(
"Failed to power on the robot, and cannot complete navigate route request."
)
return
# Stop the lease keepalive and create a new sublease for graph nav.
self._lease = self._lease_wallet.advance()
sublease = self._lease.create_sublease()
self._lease_keepalive.shutdown()
# Navigate a specific route.
route = self._graph_nav_client.build_route(waypoint_ids,
edge_ids_list)
is_finished = False
while not is_finished:
# Issue the route command about twice a second such that it is easy to terminate the
# navigation command (with estop or killing the program).
nav_route_command_id = self._graph_nav_client.navigate_route(
route, cmd_duration=1.0, leases=[sublease.lease_proto])
time.sleep(
.5
) # Sleep for half a second to allow for command execution.
# Poll the robot for feedback to determine if the route is complete. Then sit
# the robot down once it is finished.
is_finished = self._check_success(nav_route_command_id)
self._lease = self._lease_wallet.advance()
self._lease_keepalive = LeaseKeepAlive(self._lease_client)
# Update the lease and power off the robot if appropriate.
if self._powered_on and not self._started_powered_on:
# Sit the robot down + power off after the navigation command is complete.
self.toggle_power(should_power_on=False)
def _clear_graph(self, *args):
"""Clear the state of the map on the robot, removing all waypoints and edges."""
return self._graph_nav_client.clear_graph(
lease=self._lease.lease_proto)
def toggle_power(self, should_power_on):
"""Power the robot on/off dependent on the current power state."""
is_powered_on = self.check_is_powered_on()
if not is_powered_on and should_power_on:
# Power on the robot up before navigating when it is in a powered-off state.
power_on(self._power_client)
motors_on = False
while not motors_on:
future = self._robot_state_client.get_robot_state_async()
state_response = future.result(
timeout=10
) # 10 second timeout for waiting for the state response.
if state_response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_ON:
motors_on = True
else:
# Motors are not yet fully powered on.
time.sleep(.25)
elif is_powered_on and not should_power_on:
# Safe power off (robot will sit then power down) when it is in a
# powered-on state.
safe_power_off(self._robot_command_client,
self._robot_state_client)
else:
# Return the current power state without change.
return is_powered_on
# Update the locally stored power state.
self.check_is_powered_on()
return self._powered_on
def check_is_powered_on(self):
"""Determine if the robot is powered on or off."""
power_state = self._robot_state_client.get_robot_state().power_state
self._powered_on = (
power_state.motor_power_state == power_state.STATE_ON)
return self._powered_on
def _check_success(self, command_id=-1):
"""Use a navigation command id to get feedback from the robot and sit when command succeeds."""
if command_id == -1:
# No command, so we have not status to check.
return False
status = self._graph_nav_client.navigation_feedback(command_id)
if status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_REACHED_GOAL:
# Successfully completed the navigation commands!
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_LOST:
self._logger.error(
"Robot got lost when navigating the route, the robot will now sit down."
)
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_STUCK:
self._logger.error(
"Robot got stuck when navigating the route, the robot will now sit down."
)
return True
elif status.status == graph_nav_pb2.NavigationFeedbackResponse.STATUS_ROBOT_IMPAIRED:
self._logger.error("Robot is impaired.")
return True
else:
# Navigation command is not complete yet.
return False
def _match_edge(self, current_edges, waypoint1, waypoint2):
"""Find an edge in the graph that is between two waypoint ids."""
# Return the correct edge id as soon as it's found.
for edge_to_id in current_edges:
for edge_from_id in current_edges[edge_to_id]:
if (waypoint1 == edge_to_id) and (waypoint2 == edge_from_id):
# This edge matches the pair of waypoints! Add it the edge list and continue.
return map_pb2.Edge.Id(from_waypoint=waypoint2,
to_waypoint=waypoint1)
elif (waypoint2 == edge_to_id) and (waypoint1 == edge_from_id):
# This edge matches the pair of waypoints! Add it the edge list and continue.
return map_pb2.Edge.Id(from_waypoint=waypoint1,
to_waypoint=waypoint2)
return None