def test_upload_edge_exceptions(client, service, server):
make_call = lambda: client.upload_edge_snapshot(map_pb2.EdgeSnapshot())
make_call()
service.lease_use_result = lease_pb2.LeaseUseResult(
status=lease_pb2.LeaseUseResult.STATUS_OLDER)
with pytest.raises(bosdyn.client.LeaseUseError):
make_call()
def _upload_graph_and_snapshots(self, *args):
"""Upload the graph and snapshots to the robot."""
print("Loading the graph from disk into local storage...")
with open(self._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)
print("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(
self._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:
if len(edge.snapshot_id) == 0:
continue
# Load the edge snapshots from disk.
with open(
self._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.
print("Uploading the graph and snapshots to the robot...")
true_if_empty = not len(self._current_graph.anchoring.anchors)
response = self._graph_nav_client.upload_graph(
lease=self._lease.lease_proto,
graph=self._current_graph,
generate_new_anchoring=true_if_empty)
# Upload the snapshots to the robot.
for snapshot_id in response.unknown_waypoint_snapshot_ids:
waypoint_snapshot = self._current_waypoint_snapshots[snapshot_id]
self._graph_nav_client.upload_waypoint_snapshot(waypoint_snapshot)
print("Uploaded {}".format(waypoint_snapshot.id))
for snapshot_id in response.unknown_edge_snapshot_ids:
edge_snapshot = self._current_edge_snapshots[snapshot_id]
self._graph_nav_client.upload_edge_snapshot(edge_snapshot)
print("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.
print("\n")
print("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 upload_graph_and_snapshots(robot, client, lease, path, disable_alternate_route_finding):
'''Upload the graph and snapshots to the robot'''
# Load the graph from disk.
graph_filename = os.path.join(path, 'graph')
robot.logger.info('Loading graph from ' + graph_filename)
with open(graph_filename, 'rb') as graph_file:
data = graph_file.read()
current_graph = map_pb2.Graph()
current_graph.ParseFromString(data)
robot.logger.info('Loaded graph has {} waypoints and {} edges'.format(
len(current_graph.waypoints), len(current_graph.edges)))
if disable_alternate_route_finding:
for edge in current_graph.edges:
edge.annotations.disable_alternate_route_finding = True
# Load the waypoint snapshots from disk.
current_waypoint_snapshots = dict()
for waypoint in current_graph.waypoints:
snapshot_filename = os.path.join(path, 'waypoint_snapshots', waypoint.snapshot_id)
robot.logger.info('Loading waypoint snapshot from ' + snapshot_filename)
with open(snapshot_filename, 'rb') as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
current_waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot
# Load the edge snapshots from disk.
current_edge_snapshots = dict()
for edge in current_graph.edges:
snapshot_filename = os.path.join(path, 'edge_snapshots', edge.snapshot_id)
robot.logger.info('Loading edge snapshot from ' + snapshot_filename)
with open(snapshot_filename, 'rb') as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
current_edge_snapshots[edge_snapshot.id] = edge_snapshot
# Upload the graph to the robot.
robot.logger.info('Uploading the graph and snapshots to the robot...')
response = client.upload_graph(graph=current_graph, lease=lease)
robot.logger.info('Uploaded graph.')
# Upload the snapshots to the robot.
for snapshot_id in response.unknown_waypoint_snapshot_ids:
waypoint_snapshot = current_waypoint_snapshots[snapshot_id]
client.upload_waypoint_snapshot(waypoint_snapshot=waypoint_snapshot, lease=lease)
robot.logger.info('Uploaded {}'.format(waypoint_snapshot.id))
for snapshot_id in response.unknown_edge_snapshot_ids:
edge_snapshot = current_edge_snapshots[snapshot_id]
client.upload_edge_snapshot(edge_snapshot=edge_snapshot, lease=lease)
robot.logger.info('Uploaded {}'.format(edge_snapshot.id))
def upload_graph_and_snapshots(robot, client, lease, path):
'''Upload the graph and snapshots to the robot'''
# Load the graph from disk.
graph_filename = os.path.join(path, 'graph')
robot.logger.info('Loading graph from ' + graph_filename)
with open(graph_filename, 'rb') as graph_file:
data = graph_file.read()
current_graph = map_pb2.Graph()
current_graph.ParseFromString(data)
robot.logger.info('Loaded graph has {} waypoints and {} edges'.format(
len(current_graph.waypoints), len(current_graph.edges)))
# Load the waypoint snapshots from disk.
current_waypoint_snapshots = dict()
for waypoint in current_graph.waypoints:
snapshot_filename = os.path.join(path, 'waypoint_snapshots',
waypoint.snapshot_id)
robot.logger.info('Loading waypoint snapshot from ' +
snapshot_filename)
with open(snapshot_filename, 'rb') as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
current_waypoint_snapshots[
waypoint_snapshot.id] = waypoint_snapshot
# Load the edge snapshots from disk.
current_edge_snapshots = dict()
for edge in current_graph.edges:
snapshot_filename = os.path.join(path, 'edge_snapshots',
edge.snapshot_id)
robot.logger.info('Loading edge snapshot from ' + snapshot_filename)
with open(snapshot_filename, 'rb') as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
current_edge_snapshots[edge_snapshot.id] = edge_snapshot
# Upload the graph to the robot.
robot.logger.info('Uploading the graph and snapshots to the robot...')
client.upload_graph(graph=current_graph, lease=lease)
robot.logger.info('Uploaded graph.')
# Upload the snapshots to the robot.
for waypoint_snapshot in current_waypoint_snapshots.values():
client.upload_waypoint_snapshot(waypoint_snapshot=waypoint_snapshot,
lease=lease)
robot.logger.info('Uploaded {}'.format(waypoint_snapshot.id))
for edge_snapshot in current_edge_snapshots.values():
client.upload_edge_snapshot(edge_snapshot=edge_snapshot, lease=lease)
robot.logger.info('Uploaded {}'.format(edge_snapshot.id))
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 _get_streamed_edge_snapshot(response):
"""Reads a streamed response to recreate a edge snapshot."""
data = ''
num_chunks = 0
for resp in response:
if num_chunks == 0:
data = resp.chunk.data
else:
data += resp.chunk.data
num_chunks += 1
edge_snapshot = map_pb2.EdgeSnapshot()
if (num_chunks > 0):
edge_snapshot.ParseFromString(data)
return edge_snapshot
def load_map(path):
"""
Load a map from the given file path.
:param path: Path to the root directory of the map.
:return: the graph, waypoints, waypoint snapshots and edge snapshots.
"""
with open(path + "/graph", "rb") as graph_file:
# Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the
# edges between them.
data = graph_file.read()
current_graph = map_pb2.Graph()
current_graph.ParseFromString(data)
# Set up maps from waypoint ID to waypoints, edges, snapshots, etc.
current_waypoints = {}
current_waypoint_snapshots = {}
current_edge_snapshots = {}
# For each waypoint, load any snapshot associated with it.
for waypoint in current_graph.waypoints:
current_waypoints[waypoint.id] = waypoint
# Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large.
file_name = path + "/waypoint_snapshots/{}".format(
waypoint.snapshot_id)
if not os.path.exists(file_name):
continue
with open(file_name, "rb") as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
current_waypoint_snapshots[
waypoint_snapshot.id] = waypoint_snapshot
# Similarly, edges have snapshot data.
for edge in current_graph.edges:
file_name = path + "/edge_snapshots/{}".format(edge.snapshot_id)
if not os.path.exists(file_name):
continue
with open(file_name, "rb") as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
current_edge_snapshots[edge_snapshot.id] = edge_snapshot
print("Loaded graph with {} waypoints and {} edges".format(
len(current_graph.waypoints), len(current_graph.edges)))
return (current_graph, current_waypoints, current_waypoint_snapshots,
current_edge_snapshots)
def load_graph_and_snapshots(filepath):
"""
Load the graph and snapshots from a directory.
:param filepath: The full file path to the directory.
:return: a tuple containing the graph, waypoint snapshots and edge snapshots.
"""
print("Loading the graph from disk into local storage at {}".format(
filepath))
graph = map_pb2.Graph()
waypoint_snapshots = {}
edge_snapshots = {}
with open(os.path.join(filepath, "graph"), "rb") as graph_file:
# Load the graph from disk.
data = graph_file.read()
graph.ParseFromString(data)
print("Loaded graph has {} waypoints and {} edges".format(
len(graph.waypoints), len(graph.edges)))
for waypoint in graph.waypoints:
if len(waypoint.snapshot_id) == 0:
continue
# Load the waypoint snapshots from disk.
with open(
os.path.join(filepath, "waypoint_snapshots",
waypoint.snapshot_id), "rb") as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
waypoint_snapshots[waypoint_snapshot.id] = waypoint_snapshot
for edge in graph.edges:
if len(edge.snapshot_id) == 0:
continue
# Load the edge snapshots from disk.
with open(os.path.join(filepath, "edge_snapshots", edge.snapshot_id),
"rb") as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
edge_snapshots[edge_snapshot.id] = edge_snapshot
return (graph, waypoint_snapshots, edge_snapshots)
def load_map(path):
"""
Load a map from the given file path.
:param path: Path to the root directory of the map.
:return: the graph, waypoints, waypoint snapshots, edge snapshots, and anchorings.
"""
with open(os.path.join(path, "graph"), "rb") as graph_file:
# Load the graph file and deserialize it. The graph file is a protobuf containing only the waypoints and the
# edges between them.
data = graph_file.read()
current_graph = map_pb2.Graph()
current_graph.ParseFromString(data)
# Set up maps from waypoint ID to waypoints, edges, snapshots, etc.
current_waypoints = {}
current_waypoint_snapshots = {}
current_edge_snapshots = {}
current_anchors = {}
current_anchored_world_objects = {}
# Load the anchored world objects first so we can look in each waypoint snapshot as we load it.
for anchored_world_object in current_graph.anchoring.objects:
current_anchored_world_objects[anchored_world_object.id] = (
anchored_world_object, )
# For each waypoint, load any snapshot associated with it.
for waypoint in current_graph.waypoints:
current_waypoints[waypoint.id] = waypoint
if len(waypoint.snapshot_id) == 0:
continue
# Load the snapshot. Note that snapshots contain all of the raw data in a waypoint and may be large.
file_name = os.path.join(path, "waypoint_snapshots",
waypoint.snapshot_id)
if not os.path.exists(file_name):
continue
with open(file_name, "rb") as snapshot_file:
waypoint_snapshot = map_pb2.WaypointSnapshot()
waypoint_snapshot.ParseFromString(snapshot_file.read())
current_waypoint_snapshots[
waypoint_snapshot.id] = waypoint_snapshot
for fiducial in waypoint_snapshot.objects:
if not fiducial.HasField("apriltag_properties"):
continue
str_id = str(fiducial.apriltag_properties.tag_id)
if (str_id in current_anchored_world_objects and len(
current_anchored_world_objects[str_id]) == 1):
# Replace the placeholder tuple with a tuple of (wo, waypoint, fiducial).
anchored_wo = current_anchored_world_objects[str_id][0]
current_anchored_world_objects[str_id] = (anchored_wo,
waypoint,
fiducial)
# Similarly, edges have snapshot data.
for edge in current_graph.edges:
if len(edge.snapshot_id) == 0:
continue
file_name = os.path.join(path, "edge_snapshots", edge.snapshot_id)
if not os.path.exists(file_name):
continue
with open(file_name, "rb") as snapshot_file:
edge_snapshot = map_pb2.EdgeSnapshot()
edge_snapshot.ParseFromString(snapshot_file.read())
current_edge_snapshots[edge_snapshot.id] = edge_snapshot
for anchor in current_graph.anchoring.anchors:
current_anchors[anchor.id] = anchor
print(
"Loaded graph with {} waypoints, {} edges, {} anchors, and {} anchored world objects"
.format(len(current_graph.waypoints), len(current_graph.edges),
len(current_graph.anchoring.anchors),
len(current_graph.anchoring.objects)))
return (current_graph, current_waypoints, current_waypoint_snapshots,
current_edge_snapshots, current_anchors,
current_anchored_world_objects)
