def test_upload_graph_exceptions(client, service, server): make_call = lambda: client.upload_graph(graph=map_pb2.Graph()) 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 __init__(self): super(MockMapProcessingServicer, self).__init__() self.common_header_code = header_pb2.CommonError.CODE_OK self.topology_process_status = map_processing_pb2.ProcessTopologyResponse.STATUS_OK self.anchoring_process_status = map_processing_pb2.ProcessAnchoringResponse.STATUS_OK self.graph = map_pb2.Graph() ed = self.graph.edges.add() ed.id.from_waypoint = 'w1' ed.id.to_waypoint = 'w2'
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 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 test_upload_graph_exceptions(client, service, server): make_call = lambda: client.upload_graph(graph=map_pb2.Graph()) make_call() service.lease_use_result = lease_pb2.LeaseUseResult( status=lease_pb2.LeaseUseResult.STATUS_OLDER) with pytest.raises(bosdyn.client.LeaseUseError): make_call() service.lease_use_result = lease_pb2.LeaseUseResult( status=lease_pb2.LeaseUseResult.STATUS_OK) make_call() service.upload_graph_resp.status = service.upload_graph_resp.STATUS_MAP_TOO_LARGE_LICENSE with pytest.raises(bosdyn.client.graph_nav.MapTooLargeLicenseError): make_call() service.upload_graph_resp.status = service.upload_graph_resp.STATUS_INVALID_GRAPH with pytest.raises(bosdyn.client.graph_nav.InvalidGraphError): make_call()
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)