diff --git a/openpilot/selfdrive/navigation/auth.py b/openpilot/selfdrive/navigation/auth.py deleted file mode 100644 index f06ddecd3..000000000 --- a/openpilot/selfdrive/navigation/auth.py +++ /dev/null @@ -1,43 +0,0 @@ -from __future__ import annotations - -import os -from pathlib import Path - -from openpilot.common.params import Params, UnknownKeyName - -NAV_API_HOST = os.environ.get("NAV_API_HOST", "https://nav.mr-one.cn").rstrip("/") -NAV_TOKEN_PATH = Path(os.environ.get("NAV_TOKEN_PATH", "/data/nav_token")) -NAV_API_HOST_FILE = Path(os.environ.get("NAV_API_HOST_FILE", "/data/nav_api_host")) -NAV_DEVICE_ID = os.environ.get("NAV_DEVICE_ID", "") - - -def _as_text(value) -> str: - if isinstance(value, bytes): - return value.decode("utf-8", "replace").strip() - return str(value or "").strip() - - -def nav_api_host(params: Params | None = None) -> str: - params = params if params is not None else Params() - try: - override = _as_text(params.get("NavApiHost")) - except UnknownKeyName: - override = "" - if not override: - try: - override = NAV_API_HOST_FILE.read_text(encoding="utf-8").strip() - except OSError: - pass - return (override or NAV_API_HOST).rstrip("/") - - -def nav_auth(params: Params | None = None) -> tuple[str, dict[str, str]]: - params = params if params is not None else Params() - device_id = _as_text(NAV_DEVICE_ID or params.get("DongleId")) - try: - token = NAV_TOKEN_PATH.read_text(encoding="utf-8").strip() - except OSError: - token = "" - if not device_id or not token: - raise RuntimeError("navigation credentials are not installed") - return device_id, {"X-Device-ID": device_id, "X-Nav-Token": token} diff --git a/openpilot/selfdrive/navigation/mapsd.py b/openpilot/selfdrive/navigation/mapsd.py deleted file mode 100755 index 729e21b38..000000000 --- a/openpilot/selfdrive/navigation/mapsd.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 -import time - -import numpy as np - -import openpilot.cereal.messaging as messaging -from msgq.visionipc import VisionIpcServer -from openpilot.cereal.visionipc import VisionStreamType -from openpilot.common.realtime import Ratekeeper -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.navigation.helpers import latest_gps_source -from openpilot.selfdrive.navigation.pmtiles import PMTiles -from openpilot.selfdrive.navigation.renderer import SIZE, MapRenderer -from openpilot.selfdrive.navigation.route import RouteTracker -from openpilot.selfdrive.navigation.tiles import RemoteTiles - -RETRY_SECONDS = 10 - - -class MapsD: - def __init__(self): - self.renderer = None - self.tracker = None - self.route = np.empty((0, 2)) - self.prefetched = False - self.retry_at = 0.0 - - def update(self, sm) -> tuple[np.ndarray, int]: - """The map frame for the latest messages: black with a zero timestamp without a usable map, never stale.""" - if sm.updated['navRoute']: - route = np.array([(c.longitude, c.latitude) for c in sm['navRoute'].coordinates], dtype=np.float64).reshape((-1, 2)) - if not np.array_equal(route, self.route): - self.route = route - self.tracker = RouteTracker(route) if len(route) >= 2 else None # progress along the route starts over - self.prefetched = False - now = time.monotonic() - source = latest_gps_source(sm) - try: - if self.tracker is not None and now >= self.retry_at: - if self.renderer is None: - self.renderer = MapRenderer(PMTiles(RemoteTiles())) - if source is not None: - gps = sm[source] - # As in training, the map is centered on the route and headed along it, not at the GPS fix. - pose = self.tracker.snap(gps.longitude, gps.latitude) - # Render-time reads fetch only the tiles actually needed. Avoid batching ranges: long merged - # requests are unreliable on constrained links, while individual tiles are cached on disk. - self.prefetched = True - image, valid = self.renderer.render(*pose, self.route) - if valid: - return image, sm.logMonoTime[source] - except Exception: # a failed fetch, or a corrupt cached tile - cloudlog.exception('mapsd.render_failed') - self.close() # a changed archive needs a fresh reader - self.retry_at = now + RETRY_SECONDS - return np.zeros((SIZE, SIZE), dtype=np.uint8), 0 - - def close(self): - if self.renderer is not None: - self.renderer.archive.close() - self.renderer = None - self.prefetched = False - - -def main(): - rk = Ratekeeper(20.0) - sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'navRoute']) - server = VisionIpcServer('mapsd') - server.create_buffers_with_sizes(VisionStreamType.VISION_STREAM_MAP, 4, SIZE, SIZE, SIZE * SIZE, SIZE, SIZE * SIZE) - server.start_listener() - mapsd = MapsD() - try: - while True: - sm.update(0) - image, timestamp = mapsd.update(sm) - server.send(VisionStreamType.VISION_STREAM_MAP, image.tobytes(), rk.frame, timestamp, time.monotonic_ns()) - rk.keep_time() - finally: - mapsd.close() - - -if __name__ == '__main__': - main() diff --git a/openpilot/selfdrive/navigation/navd.py b/openpilot/selfdrive/navigation/navd.py deleted file mode 100755 index 96f86d6b5..000000000 --- a/openpilot/selfdrive/navigation/navd.py +++ /dev/null @@ -1,236 +0,0 @@ -#!/usr/bin/env python3 -import os -import time - -import numpy as np -import requests - -import openpilot.cereal.messaging as messaging -from openpilot.common.params import Params -from openpilot.common.realtime import Ratekeeper -from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.navigation.auth import nav_api_host, nav_auth -from openpilot.selfdrive.navigation.helpers import latest_gps_source -from openpilot.selfdrive.navigation.route import along_path, local_meters - -REROUTE_DISTANCE = 25 -MANEUVER_TRANSITION_THRESHOLD = 10 -REROUTE_COUNTER_MIN = 3 -ROUTE_RESEND_PERIOD = 10 # updates (seconds); a restarted mapsd gets the route within this -RECOMPUTE_HOLD = 10 # updates (seconds) between route requests - - -def set_banner(instruction, banners, distance_to_maneuver): - """Fill the banner fields from the banner to show now: the last one still ahead of the ego, else the first.""" - if not banners: - return - banner = next((b for b in reversed(banners) if distance_to_maneuver < b['distanceAlongGeometry']), banners[0]) - instruction.showFull = distance_to_maneuver < banner['distanceAlongGeometry'] - primary = banner['primary'] - instruction.maneuverPrimaryText = primary.get('text') or '' - instruction.maneuverType = primary.get('type') or '' - instruction.maneuverModifier = primary.get('modifier') or '' - if banner.get('secondary') is not None: - instruction.maneuverSecondaryText = banner['secondary']['text'] - - -class RouteEngine: - def __init__(self, sm, pm, params=None): - self.sm = sm - self.pm = pm - self.params = params if params is not None else Params() - - self.last_position = None # (longitude, latitude) - self.last_bearing = None - - self.nav_destination = None - self.step_idx = None - self.route = None - self.route_geometry = None # an (n, 2) array of (longitude, latitude) per step - - self.recompute_hold = 0 - self.reroute_counter = 0 - - self.mapbox_token = os.environ.get("MAPBOX_TOKEN") - self.mapbox_host = "https://api.mapbox.com" if self.mapbox_token is not None else nav_api_host(self.params) + '/v1/nav' - - def update(self): - self.sm.update(0) - - source = latest_gps_source(self.sm) - if source is not None: - gps = self.sm[source] - self.last_position, self.last_bearing = (gps.longitude, gps.latitude), gps.bearingDeg - route = self.route - try: - self.recompute_route(gps_ok=source is not None) - self.send_instruction() - except Exception: - cloudlog.exception("navd.failed_to_compute") - # mapsd dedupes repeats, so a periodic resend is how a restarted mapsd gets the route - if self.route is not route or self.sm.frame % ROUTE_RESEND_PERIOD == 0: - self.send_route() - - def recompute_route(self, gps_ok: bool): - if self.last_position is None: - return - - destination = self.params.get("NavDestination") - if destination is None: - self.clear_route() - self.recompute_hold = 0 - return - new_destination = (destination['longitude'], destination['latitude']) - - # Don't recompute when GPS drifts in tunnels - if not gps_ok and self.step_idx is not None: - return - - self.recompute_hold -= 1 - if (self.should_recompute() or new_destination != self.nav_destination) and self.recompute_hold <= 0: - self.recompute_hold = RECOMPUTE_HOLD - self.calculate_route(new_destination) - self.reroute_counter = 0 - - def calculate_route(self, destination): - assert self.last_position is not None - cloudlog.warning(f"Calculating route {self.last_position} -> {destination}") - self.nav_destination = destination - - lang = self.params.get('LanguageSetting') - if lang is not None: - lang = lang.replace('main_', '') - - params = { - 'geometries': 'geojson', - 'overview': 'full', - 'steps': 'true', - 'banner_instructions': 'true', - 'alternatives': 'false', - 'language': lang, - } - if self.last_bearing is not None: - params['bearings'] = f"{(self.last_bearing + 360) % 360:.0f},90;" - - coords_str = ';'.join(f'{lon},{lat}' for lon, lat in (self.last_position, destination)) - url = self.mapbox_host + '/directions/v5/mapbox/driving-traffic/' + coords_str - try: - headers = None - if self.mapbox_token is not None: - params['access_token'] = self.mapbox_token - else: - _, headers = nav_auth(self.params) - resp = None - for attempt in range(5): - try: - resp = requests.get(url, params=params, headers=headers, timeout=15) - if resp.status_code == 200: - break - except requests.exceptions.RequestException: - if attempt == 4: - raise - if attempt < 4: - time.sleep(0.4 * (2 ** attempt)) - assert resp is not None - if resp.status_code != 200: - cloudlog.event("API request failed", status_code=resp.status_code, text=resp.text, error=True) - resp.raise_for_status() - - r = resp.json() - if len(r['routes']): - self.route = r['routes'][0]['legs'][0]['steps'] - self.route_geometry = [np.array(step['geometry']['coordinates'], dtype=np.float64).reshape((-1, 2)) for step in self.route] - self.step_idx = 0 - else: - cloudlog.warning("Got empty route response") - self.clear_route() - - except (requests.exceptions.RequestException, RuntimeError): - cloudlog.exception("failed to get route") - self.clear_route() - - def send_instruction(self): - msg = messaging.new_message('navInstruction', valid=True) - - if self.step_idx is None: - msg.valid = False - self.pm.send('navInstruction', msg) - return - - assert self.route is not None and self.route_geometry is not None and self.last_position is not None - step = self.route[self.step_idx] - along_geometry, _ = along_path(self.route_geometry[self.step_idx], self.last_position) - distance_to_maneuver_along_geometry = step['distance'] - along_geometry - - # Banner instructions are for the following maneuver step, don't use empty last step - banner_step = step - if not len(banner_step['bannerInstructions']) and self.step_idx == len(self.route) - 1: - banner_step = self.route[max(self.step_idx - 1, 0)] - - # Current instruction - msg.navInstruction.maneuverDistance = distance_to_maneuver_along_geometry - set_banner(msg.navInstruction, banner_step['bannerInstructions'], distance_to_maneuver_along_geometry) - - # Total remaining distance and time: the rest of this step, then every step after it - remaining = 1.0 - along_geometry / max(step['distance'], 1) - future = self.route[self.step_idx + 1:] - msg.navInstruction.distanceRemaining = step['distance'] * remaining + sum(s['distance'] for s in future) - msg.navInstruction.timeRemaining = step['duration'] * remaining + sum(s['duration'] for s in future) - - self.pm.send('navInstruction', msg) - - # Transition to next route segment - if distance_to_maneuver_along_geometry < -MANEUVER_TRANSITION_THRESHOLD: - if self.step_idx + 1 < len(self.route): - self.step_idx += 1 - self.recompute_hold = 0 - else: - cloudlog.warning("Destination reached") - - # Clear route if driving away from destination - if np.linalg.norm(local_meters(self.last_position, self.nav_destination)) > REROUTE_DISTANCE: - self.params.remove("NavDestination") - self.clear_route() - - def send_route(self): - coords = np.concatenate(self.route_geometry) if self.route_geometry is not None else np.empty((0, 2)) - msg = messaging.new_message('navRoute', valid=True) - msg.navRoute.coordinates = [{'longitude': lon, 'latitude': lat} for lon, lat in coords.tolist()] - self.pm.send('navRoute', msg) - - def clear_route(self): - self.route = None - self.route_geometry = None - self.step_idx = None - self.nav_destination = None - - def should_recompute(self): - if self.step_idx is None: - return True - - assert self.route is not None and self.route_geometry is not None and self.last_position is not None - # Don't recompute in last segment, assume destination is reached - if self.step_idx == len(self.route) - 1: - return False - - # TODO: Check for going wrong way in segment - if along_path(self.route_geometry[self.step_idx], self.last_position)[1] > REROUTE_DISTANCE: - self.reroute_counter += 1 - else: - self.reroute_counter = 0 - return self.reroute_counter > REROUTE_COUNTER_MIN - - -def main(): - pm = messaging.PubMaster(['navInstruction', 'navRoute']) - sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) - - rk = Ratekeeper(1.0) - route_engine = RouteEngine(sm, pm) - while True: - route_engine.update() - rk.keep_time() - - -if __name__ == "__main__": - main() diff --git a/openpilot/selfdrive/navigation/tests/test_navigation.py b/openpilot/selfdrive/navigation/tests/test_navigation.py deleted file mode 100644 index b08df935b..000000000 --- a/openpilot/selfdrive/navigation/tests/test_navigation.py +++ /dev/null @@ -1,557 +0,0 @@ -import gzip -import json -import re -import struct -import tempfile -import threading -import time -import unittest -from contextlib import closing, contextmanager -from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer -from pathlib import Path -from types import SimpleNamespace -from unittest.mock import MagicMock, patch - -import numpy as np -import requests - -import openpilot.cereal.messaging as messaging -from openpilot.common.prefix import OpenpilotPrefix -from openpilot.selfdrive.navigation import auth as nav_auth_module -from openpilot.selfdrive.navigation.mapsd import MapsD -from openpilot.selfdrive.navigation.navd import REROUTE_COUNTER_MIN, ROUTE_RESEND_PERIOD, RouteEngine, set_banner -from openpilot.selfdrive.navigation.pmtiles import PMTiles, decode_directory, tile_id -from openpilot.selfdrive.navigation.renderer import MAJOR, MEDIUM, MINOR, SIZE, STREET, TRACK, MapRenderer, draw_segments, mercator, project, stroke_widths -from openpilot.selfdrive.navigation.route import RouteTracker -from openpilot.selfdrive.navigation.tiles import RemoteTiles -from openpilot.selfdrive.navigation.vector_tile import decode_lines, decode_roads - -METER = 1 / 111319.49079327358 # degrees of latitude - - -def draw_line(image, points, intensity, width, **kwargs): - draw_segments(image, points[:-1], points[1:], width / 2, intensity, **kwargs) - - -def varint(value): - output = bytearray() - while value > 127: - output.append((value & 127) | 128) - value >>= 7 - output.append(value) - return bytes(output) - - -def field(number, value): - if isinstance(value, bytes): - return varint(number << 3 | 2) + varint(len(value)) + value - return varint(number << 3) + varint(value) - - -def road_tile(crossing=None): - # One vertical line, from (2048, 0) to (2048, 4096), and one non-road layer. With crossing='bridge' or 'tunnel', - # a horizontal line from (1024, 2048) to (3072, 2048) at that level. - geometry = b''.join(varint(v) for v in [9, 4096, 0, 10, 0, 8192]) - feature = field(2, b'\x00\x00') + field(3, 2) + field(4, geometry) - roads = field(15, 2) + field(1, b'roads') + field(2, feature) - if crossing is not None: - geometry = b''.join(varint(v) for v in [9, 2048, 4096, 10, 4096, 0]) - roads += field(2, field(2, b'\x00\x00\x01\x01') + field(3, 2) + field(4, geometry)) - roads += field(3, b'kind') + field(3, f'is_{crossing}'.encode()) + field(4, field(1, b'minor_road')) + field(4, field(7, 1)) + field(5, 4096) - return field(3, field(1, b'buildings')) + field(3, roads) - - -def make_archive(payload=None, leaf=False, run=1, max_zoom=0): - """A PMTiles archive with one zoom-zero tile, in the root directory or in a leaf directory.""" - tile = gzip.compress(road_tile() if payload is None else payload) - directory = gzip.compress(varint(1) + varint(0) + varint(run) + varint(len(tile)) + varint(1)) - child = directory if leaf else b'' - root = gzip.compress(varint(1) + varint(0) + varint(0) + varint(len(child)) + varint(1)) if leaf else directory - metadata = gzip.compress(b'{"vector_layers":[{"id":"roads"}]}') - header = bytearray(127) - header[:8] = b'PMTiles\x03' - offsets = [127, 127 + len(root), 127 + len(root) + len(metadata), 127 + len(root) + len(metadata) + len(child)] - struct.pack_into('<11Q', header, 8, offsets[0], len(root), offsets[1], len(metadata), offsets[2], len(child), offsets[3], len(tile), 1, 1, 1) - header[96:102] = bytes([1, 2, 2, 1, 0, max_zoom]) - return bytes(header + root + metadata + child + tile) - - -@contextmanager -def local_archive(data): - """A PMTiles reader over in-memory bytes, bypassing the tile server.""" - source = MagicMock(size=len(data)) - source.read.side_effect = lambda offset, length: data[offset:offset + length] - with closing(PMTiles(source)) as archive: - yield archive - - -def mapbox_response(steps): - response = MagicMock(status_code=200) - response.json.return_value = {'routes': [{'legs': [{'steps': steps}]}]} - return response - - -def route_engine(testcase, sm, pm, values): - params = MagicMock() - params.get.side_effect = values.get - params.remove.side_effect = lambda key: values.pop(key, None) - auth_patch = patch('openpilot.selfdrive.navigation.navd.nav_auth', - return_value=('test-dongle', {'X-Device-ID': 'test-dongle', 'X-Nav-Token': 'device-token'})) - auth_patch.start() - testcase.addCleanup(auth_patch.stop) - host_patch = patch('openpilot.selfdrive.navigation.navd.nav_api_host', return_value='https://nav.mr-one.cn') - host_patch.start() - testcase.addCleanup(host_patch.stop) - return RouteEngine(sm, pm, params=params) - - -class TestNavAuth(unittest.TestCase): - def test_api_host_default_and_override(self): - params = MagicMock() - params.get.return_value = None - with tempfile.TemporaryDirectory() as directory: - with patch.object(nav_auth_module, 'NAV_API_HOST', 'https://nav.example/'): - with patch.object(nav_auth_module, 'NAV_API_HOST_FILE', Path(directory) / 'missing'): - self.assertEqual(nav_auth_module.nav_api_host(params), 'https://nav.example') - host_file = Path(directory) / 'nav_api_host' - host_file.write_text('http://192.168.3.203:5051/\n', encoding='utf-8') - params.get.side_effect = nav_auth_module.UnknownKeyName('NavApiHost') - with patch.object(nav_auth_module, 'NAV_API_HOST_FILE', host_file): - self.assertEqual(nav_auth_module.nav_api_host(params), 'http://192.168.3.203:5051') - params.get.side_effect = None - params.get.return_value = b'http://192.168.3.204:5051/' - self.assertEqual(nav_auth_module.nav_api_host(params), 'http://192.168.3.204:5051') - - def test_reads_device_token(self): - with tempfile.TemporaryDirectory() as directory: - token_path = Path(directory) / 'nav_token' - token_path.write_text('nav_test_token', encoding='utf-8') - params = MagicMock() - params.get.return_value = 'test-dongle' - with patch.object(nav_auth_module, 'NAV_TOKEN_PATH', token_path): - with patch.object(nav_auth_module, 'NAV_DEVICE_ID', ''): - device_id, headers = nav_auth_module.nav_auth(params) - self.assertEqual(device_id, 'test-dongle') - self.assertEqual(headers, {'X-Device-ID': 'test-dongle', 'X-Nav-Token': 'nav_test_token'}) - - def test_missing_token_fails_closed(self): - params = MagicMock() - params.get.return_value = 'test-dongle' - with tempfile.TemporaryDirectory() as directory: - missing = Path(directory) / 'missing' - with patch.object(nav_auth_module, 'NAV_TOKEN_PATH', missing): - with patch.object(nav_auth_module, 'NAV_DEVICE_ID', ''): - with self.assertRaisesRegex(RuntimeError, 'navigation credentials'): - nav_auth_module.nav_auth(params) - - -class RangeHandler(BaseHTTPRequestHandler): - def do_GET(self): - if self.path.startswith('/v1/'): - self.server.metadata_requests.append(dict(self.headers)) - if self.server.metadata_status != 200: - self.send_error(self.server.metadata_status) - return - data = json.dumps({'url': self.server.url, 'expires': time.time() + 86400}).encode() # noqa: TID251 - self.send_response(200) - self.send_header('Content-Type', 'application/json') - self.send_header('Content-Length', str(len(data))) - self.end_headers() - self.wfile.write(data) - return - self.server.requests.append(dict(self.headers)) - if self.headers.get('If-Match', self.server.etag) != self.server.etag: - self.send_error(412) - return - if self.server.ignore_range: - self.send_response(200) - self.end_headers() - return - offset, end = map(int, re.fullmatch(r'bytes=(\d+)-(\d+)', self.headers['Range']).groups()) - data = self.server.archive[offset:end + 1] - self.send_response(206) - self.send_header('Content-Range', f'bytes {offset}-{end}/{len(self.server.archive)}') - self.send_header('Content-Length', str(len(data))) - self.send_header('ETag', self.server.etag) - self.end_headers() - self.wfile.write(data) - - def log_message(self, format: str, *args: object) -> None: # noqa: A002 # stdlib override - pass - - -class TileServerCase(unittest.TestCase): - """Serves a leaf-directory archive over HTTP range requests, with the comma API and tile cache stubbed.""" - - def setUp(self): - self.directory = tempfile.TemporaryDirectory() - self.addCleanup(self.directory.cleanup) - self.cache = Path(self.directory.name) / 'navigation' / 'tiles' - self.server = ThreadingHTTPServer(('127.0.0.1', 0), RangeHandler) - self.server.archive = make_archive(leaf=True) - self.server.etag = '"test-version"' - self.server.ignore_range = False - self.server.requests = [] - self.server.metadata_requests = [] - self.server.metadata_status = 200 - threading.Thread(target=self.server.serve_forever, kwargs={'poll_interval': 0.01}, daemon=True).start() - self.addCleanup(self.server.server_close) - self.addCleanup(self.server.shutdown) - self.nav_auth = MagicMock(return_value=( - 'test-dongle', {'X-Device-ID': 'test-dongle', 'X-Nav-Token': 'device-token'}, - )) - self.url = f'http://127.0.0.1:{self.server.server_port}/planet.pmtiles?sig=secret' - self.server.url = self.url - patch_values = [ - ('nav_auth', self.nav_auth), - ('Params', MagicMock()), - ('PC', True), - ('nav_api_host', lambda _params: f'http://127.0.0.1:{self.server.server_port}'), - ] - for name, value in patch_values: - self.enterContext(patch(f'openpilot.selfdrive.navigation.tiles.{name}', value)) - self.enterContext(patch('openpilot.selfdrive.navigation.tiles.Paths.comma_home', return_value=self.directory.name)) - - -class TestEndToEnd(TileServerCase): - def test_navd_to_map_frames(self): - """navd routes a destination over messaging; mapsd fetches tiles over HTTP and renders map frames.""" - destination = {'latitude': 0.01, 'longitude': 0} - self.enterContext(patch('requests.get', return_value=mapbox_response([ - {'geometry': {'coordinates': [[0, -0.01], [0, 0.01]]}, 'distance': 2224, 'duration': 100, 'bannerInstructions': []}]))) - with OpenpilotPrefix(): - pm = messaging.PubMaster(['navInstruction', 'navRoute', 'gpsLocationExternal']) - sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'navRoute']) - values = {'NavDestination': dict(destination)} - navd = route_engine(self, messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']), pm, values) - mapsd = MapsD() - self.addCleanup(mapsd.close) - - def frame(fix=True): - gps = messaging.new_message('gpsLocationExternal', valid=True) - gps.gpsLocationExternal.hasFix = fix - pm.send('gpsLocationExternal', gps) - navd.update() - sm.update(0) - return mapsd.update(sm) - - image, timestamp = frame() - # The route is centered over the white road, with the ego arrow three quarters down. - self.assertEqual((image[128, 128], image[128, 131], image[188, 128], image[195, 128]), (128, 255, 255, 0)) - self.assertEqual(timestamp, sm.logMonoTime['gpsLocationExternal']) - fetched = len(self.server.requests) - tracker = mapsd.tracker - navd.send_route() # the same route again, as a restarted mapsd would get it - frame() - self.assertIs(mapsd.tracker, tracker) # progress along it is kept - values.pop('NavDestination') - self.assertFalse(frame()[0].any()) # black without a route - values['NavDestination'] = dict(destination) - self.assertEqual(frame()[0][128, 128], 128) - self.assertEqual(len(self.server.requests), fetched) # the second route is served from the cache - image, timestamp = frame(fix=False) - self.assertFalse(image.any() or timestamp) # black, not stale, without a fix - self.assertEqual(len(self.server.metadata_requests), 1) - self.assertTrue(all('Range' in headers for headers in self.server.requests)) - self.assertTrue(all(headers.get('X-Nav-Token') == 'device-token' for headers in self.server.requests)) - self.assertTrue(all(headers.get('X-Nav-Token') == 'device-token' for headers in self.server.metadata_requests)) - self.assertTrue(all(headers['If-Match'] == self.server.etag for headers in self.server.requests[1:])) - self.assertTrue(all(b'secret' not in p.read_bytes() for p in self.cache.iterdir())) - # The archive is replaced on the server and the cache evicted: the next fetch fails, the map goes black, - # and after a delay a fresh reader starts over on the new version. - self.server.etag = '"new-version"' - for cached in self.cache.iterdir(): - cached.unlink() - values.pop('NavDestination') - frame() - values['NavDestination'] = dict(destination) - image, timestamp = frame() - self.assertFalse(image.any() or timestamp) - fetched = len(self.server.requests) - self.assertFalse(frame()[1]) # not retried yet - self.assertEqual(len(self.server.requests), fetched) - mapsd.retry_at = 0 - self.assertEqual(frame()[0][128, 128], 128) - self.assertEqual((self.cache / 'version').read_text().split()[0], '"new-version"') - - -class TestRemoteTiles(TileServerCase): - def test_shared_cache_and_refresh(self): - with closing(PMTiles(RemoteTiles())) as archive: - archive.source.expires = 0 # tile metadata grants are refreshed before the next fetch - self.assertEqual(archive.get(0, 0, 0), road_tile()) - self.assertEqual(archive.get(0, 0, 0), road_tile()) - self.assertEqual(len(self.server.metadata_requests), 2) - count = len(self.server.requests) - with closing(PMTiles(RemoteTiles())) as archive: - self.assertEqual(archive.get(0, 0, 0), road_tile()) - # A second reader revalidates only the header; directories and tiles are shared on disk. - self.assertEqual(len(self.server.requests), count + 1) - with patch('openpilot.selfdrive.navigation.tiles.CACHE_LIMIT', 1), closing(RemoteTiles()) as source: - source.read(1, 2) # an uncached range is downloaded, then the cache is trimmed to the limit - self.assertEqual([p.name for p in self.cache.iterdir()], ['version']) - - def test_prefetch_merges_nearby_ranges(self): - with patch('openpilot.selfdrive.navigation.tiles.MERGE_GAP', 16), closing(RemoteTiles()) as source: - count = len(self.server.requests) - source.prefetch([(20, 10), (0, 10), (60, 10), (20, 10)]) - self.assertEqual(len(self.server.requests), count + 2) # 0-29 in one request, 60-69 in another - self.assertEqual(self.server.requests[-2]['Range'], 'bytes=0-29') - for offset in (0, 20, 60): - self.assertEqual(source.read(offset, 10), self.server.archive[offset:offset + 10]) - self.assertEqual(len(self.server.requests), count + 2) - - def test_failures(self): - with closing(PMTiles(RemoteTiles())) as archive: - self.server.etag = '"new-version"' - with self.assertRaisesRegex(OSError, '412'): # the archive changed under an open reader - archive.get(0, 0, 0) - with closing(PMTiles(RemoteTiles())) as archive: # a new reader starts over on the new version - self.assertEqual(archive.get(0, 0, 0), road_tile()) - self.server.ignore_range = True - with self.assertRaisesRegex(OSError, '200'): - RemoteTiles() - self.server.metadata_status = 401 - with self.assertRaisesRegex(OSError, '^Unable to fetch map tiles$'): # credentials stay out of the error - RemoteTiles() - - -class TestRouteEngine(unittest.TestCase): - def setUp(self): - self.pm = MagicMock() - self.values = {'NavDestination': {'latitude': 0, 'longitude': 0.002}, 'LanguageSetting': 'main_en'} - self.sm = MagicMock() - self.sm.valid = dict.fromkeys(('gpsLocation', 'gpsLocationExternal'), True) - self.sm.logMonoTime = dict.fromkeys(self.sm.valid, time.monotonic_ns()) - self.sm.recv_time = dict.fromkeys(self.sm.valid, time.monotonic()) - self.sm.__getitem__.return_value = SimpleNamespace(hasFix=True, latitude=0, longitude=0, bearingDeg=90) - self.engine = route_engine(self, self.sm, self.pm, self.values) - self.engine.last_position, self.engine.last_bearing = (0, 0), 90 - distance = 111.195 # Mapbox's length of 0.001 degrees at the equator - self.response = mapbox_response([ - {'geometry': {'coordinates': [[0, 0], [0.0005, 0], [0.001, 0]]}, 'distance': distance, 'duration': 10, - 'bannerInstructions': [{'distanceAlongGeometry': 100, 'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': 'right'}}]}, - {'geometry': {'coordinates': [[0.001, 0], [0.002, 0]]}, 'distance': distance, 'duration': 10, 'bannerInstructions': []}, - ]) - - def last_message(self, service): - return next(call.args[1] for call in reversed(self.pm.send.call_args_list) if call.args[0] == service) - - def load_route(self): - with patch('requests.get', return_value=self.response) as get: - self.engine.calculate_route((0.002, 0)) - self.engine.send_route() - return get - - def test_request_and_instruction(self): - get = self.load_route() - self.assertEqual(get.call_args.args[0], 'https://nav.mr-one.cn/v1/nav/directions/v5/mapbox/driving-traffic/0,0;0.002,0') - options = get.call_args.kwargs['params'] - self.assertNotIn('access_token', options) - self.assertEqual((options['bearings'], options['language']), ('90,90;', 'en')) - self.assertEqual(get.call_args.kwargs['headers']['X-Nav-Token'], 'device-token') - self.assertEqual(len(self.last_message('navRoute').navRoute.coordinates), 5) - self.engine.send_instruction() - message = self.last_message('navInstruction') - self.assertTrue(message.valid) - instruction = message.navInstruction.to_dict() - self.assertEqual((instruction['maneuverPrimaryText'], instruction['maneuverModifier'], instruction['timeRemaining']), ('Turn right', 'right', 20)) - self.engine.last_position = (0.0007, 0) # on the step's second segment - self.engine.send_instruction() - self.assertAlmostEqual(self.last_message('navInstruction').navInstruction.timeRemaining, 13, delta=0.05) - self.engine.last_position = (0.001, 0) # at the maneuver - self.engine.send_instruction() - self.assertAlmostEqual(self.last_message('navInstruction').navInstruction.maneuverDistance, 0, delta=0.01) - - def test_banner_selection(self): - banners = [{'distanceAlongGeometry': 100, 'primary': {'text': 'Turn right', 'type': 'turn', 'modifier': 'right'}}, - {'distanceAlongGeometry': 50, 'primary': {'text': 'Turn right'}, 'secondary': {'text': 'onto Main St'}}] - # The last banner still ahead of the ego, else the first: 120 m, 70 m and 30 m from the maneuver. - for distance, expected in [(120, ('Turn right', '', False)), (70, ('Turn right', '', True)), (30, ('Turn right', 'onto Main St', True))]: - instruction = messaging.new_message('navInstruction').navInstruction - set_banner(instruction, banners, distance) - self.assertEqual((instruction.maneuverPrimaryText, instruction.maneuverSecondaryText, instruction.showFull), expected) - - def test_route_sent_on_change_and_periodically(self): - sent = lambda: sum(call.args[0] == 'navRoute' for call in self.pm.send.call_args_list) # noqa: E731 - self.sm.frame = 1 - with patch('requests.get', return_value=self.response): - self.engine.update() # the route to a new destination is sent once - self.engine.update() - self.assertEqual(sent(), 1) - self.sm.frame = ROUTE_RESEND_PERIOD # and again periodically, for a mapsd that started after it - self.engine.update() - self.assertEqual(sent(), 2) - - def test_maneuvers_and_rerouting(self): - self.load_route() - self.engine.last_position = (0, 0.001) # 111 m off route: reroute after a few consecutive misses - for _ in range(REROUTE_COUNTER_MIN): - self.assertFalse(self.engine.should_recompute()) - self.assertTrue(self.engine.should_recompute()) - with patch('requests.get') as get: - self.engine.recompute_route(gps_ok=False) # but not while the GPS drifts in a tunnel - get.assert_not_called() - with patch('requests.get', return_value=self.response) as get: - self.engine.recompute_route(gps_ok=True) - self.engine.reroute_counter = REROUTE_COUNTER_MIN # still off route, but an immediate second recompute is held back - self.engine.recompute_route(gps_ok=True) - self.assertEqual(get.call_count, 1) - self.engine.last_position = (0.0012, 0) # past the first maneuver - self.engine.send_instruction() - self.assertEqual(self.engine.step_idx, 1) - self.engine.last_position = (0.00215, 0) # 17 m past the destination: arrived, but the route is kept nearby - self.engine.send_instruction() - self.assertIsNotNone(self.engine.route) - self.engine.last_position = (0.0024, 0) # driving away from the destination - self.engine.send_instruction() - self.assertIsNone(self.engine.route) - self.assertNotIn('NavDestination', self.values) - - def test_failed_request_clears_route(self): - self.load_route() - self.response.raise_for_status.side_effect = requests.HTTPError('test failure') - self.load_route() - self.assertIsNone(self.engine.route) - self.assertEqual(len(self.last_message('navRoute').navRoute.coordinates), 0) - self.engine.send_instruction() - self.assertFalse(self.last_message('navInstruction').valid) - - def test_gps_source(self): - fixes = {'gpsLocation': SimpleNamespace(hasFix=True, latitude=1, longitude=1, bearingDeg=180), - 'gpsLocationExternal': SimpleNamespace(hasFix=True, latitude=2, longitude=2, bearingDeg=0)} - self.sm.__getitem__.side_effect = fixes.get - self.sm.logMonoTime = {'gpsLocation': 2, 'gpsLocationExternal': 1} # replayed logs are judged by receipt time - with patch('requests.get', return_value=self.response): - self.engine.update() - self.assertEqual((self.engine.last_position, self.engine.last_bearing), ((1, 1), 180)) # the newest fix wins - self.sm.valid['gpsLocation'] = False - self.engine.update() - self.assertEqual(self.engine.last_position, (2, 2)) # unless it is invalid - fixes['gpsLocationExternal'] = SimpleNamespace(hasFix=True, latitude=3, longitude=3, bearingDeg=0) - self.sm.recv_time = dict.fromkeys(self.sm.valid, time.monotonic() - 3) # or stale - self.engine.update() - self.assertEqual(self.engine.last_position, (2, 2)) - - -class TestTiles(unittest.TestCase): - def test_pmtiles_format(self): - self.assertEqual([tile_id(1, x, y) for x, y in [(0, 0), (0, 1), (1, 1), (1, 0)]], [1, 2, 3, 4]) - self.assertEqual(sorted(tile_id(3, x, y) for x in range(8) for y in range(8)), list(range(21, 85))) - # IDs 5, 10; runs 3, 1; lengths 4, 6; the second offset follows the first blob. - data = bytes([2, 5, 5, 3, 1, 4, 6, 1, 0]) - self.assertEqual(decode_directory(data), ([5, 10], [(3, 4, 0), (1, 6, 4)])) - for invalid in (b'\x80', b'\x01\x00\x01\x01\x00', data + b'\x00'): - with self.assertRaises(ValueError): - decode_directory(invalid) - with local_archive(make_archive(run=3, max_zoom=1)) as archive: # the zoom-zero tile is also addressed by IDs 1 and 2 - self.assertEqual(archive.get(1, 0, 1), road_tile()) - self.assertIsNone(archive.get(1, 1, 1)) - with self.assertRaises(ValueError), local_archive(make_archive()[:-1]): - pass - - def test_vector_tiles(self): - roads = decode_roads(road_tile()) - self.assertEqual((len(roads), roads[0].kind), (1, 'minor_road')) - np.testing.assert_array_equal(roads[0].points, [[0.5, 0], [0.5, 1]]) - lines = decode_lines([9, 20, 20, 10, 10, 0, 9, 9, 9, 10, 0, 10]) - np.testing.assert_array_equal(lines[0], [[10, 10], [15, 10]]) - np.testing.assert_array_equal(lines[1], [[10, 5], [10, 10]]) - for data in (b'\x1a\xff', b'\x00', b'\x1a\x02\x0a\x04'): - with self.assertRaises(ValueError): - decode_roads(data) - for commands in ([10, 1, 1], [9, 0], [9, 0, 0, 15]): - with self.assertRaises(ValueError): - decode_lines(commands) - - -class TestRendering(unittest.TestCase): - def test_projection(self): - # At the equator, 64m ahead is the image center and the car is 64px below it, at 1m per pixel. - points = mercator(np.array([[0, 0], [0, 64 * METER], [64 * METER, 0]])) - np.testing.assert_allclose(project(points, 0, 0, 0), [[128, 192], [128, 128], [192, 192]], atol=1e-5) - np.testing.assert_allclose(project(points, 0, 0, 90), [[128, 192], [64, 192], [128, 128]], atol=1e-5) - pixels = project(mercator(np.array([[-179.999, 0]])), 179.999, 0, 90) # across the antimeridian - self.assertAlmostEqual(pixels[0, 1], 192 - 222.6389816, places=5) - - def test_strokes(self): - image = np.zeros((SIZE, SIZE), dtype=np.uint8) - draw_line(image, np.array([[-1000, 128], [1000, 128]]), 255, 8) - self.assertTrue(np.all(image[128] == 255)) - draw_line(image, np.array([[128, 0], [128, 256]]), 128, 5, overlay=True) # the grey route replaces the white road - self.assertEqual((image[128, 128], image[128, 131]), (128, 255)) - before = image.copy() - draw_line(image, np.array([[-1000, -1000], [-500, -500]]), 255, 5) - np.testing.assert_array_equal(image, before) - draw_line(image, np.array([[40, 40], [40, 40]]), 255, 5) # a zero-length segment is still a dot - self.assertEqual(image[40, 40], 255) - - def test_crossings(self): - route = np.array([[0.01, 0], [0.01, 0.01]]) # off to the side, out of the way of the probes - images = {} - for crossing in ('bridge', 'tunnel'): - with local_archive(make_archive(road_tile(crossing))) as archive: - images[crossing] = MapRenderer(archive).render(0, -40 * METER, 0, route)[0] # the crossing 40m ahead - # The horizontal road crosses the vertical one 40px above the ego, at row 152. A bridge severs the road beneath - # just outside its own stroke; a tunnel passes underneath untouched. The cut has butt caps, so the - # bridge's own end is not wrapped in black. - self.assertEqual((images['bridge'][146, 128], images['bridge'][152, 128], images['bridge'][152, 200], images['bridge'][140, 128]), (0, 255, 255, 255)) - self.assertEqual((images['tunnel'][146, 128], images['tunnel'][152, 200]), (255, 255)) - # The ground road cuts over the tunnel beneath it, and the tunnel is drawn without its casing. - self.assertEqual((images['tunnel'][152, 133], images['tunnel'][147, 200]), (0, 0)) - - def test_road_widths(self): - # Pinned at the style zoom of 1 m per pixel: 16.26 at the equator and 15.26 at 60 degrees. Tunnels have no casing. - styles, levels = np.array([MAJOR, MEDIUM, STREET, MINOR, TRACK, STREET]), np.array([0, 0, 0, 0, 0, -1]) - np.testing.assert_allclose(stroke_widths(16.256, styles, levels), [19.02, 15.48, 9.86, 6.70, 7.02, 7.89], atol=0.01) - np.testing.assert_allclose(stroke_widths(15.256, styles, levels), [13.52, 10.78, 6.39, 4.16, 4.58, 4.62], atol=0.01) - - def test_route_tiles(self): - route = np.array([[0, -0.01], [0, 0.01]]) # 2.2 km north along the prime meridian - with local_archive(make_archive(max_zoom=15)) as archive: - renderer = MapRenderer(archive) - tiles = renderer.route_tiles(route) - # Both columns beside the meridian, and the rows within the view radius of the route's ends. - self.assertEqual({x for x, _ in tiles}, {16383, 16384}) - self.assertEqual({y for _, y in tiles}, set(range(16382, 16386))) - - def test_validity_and_display(self): - route = np.array([[0, -0.01], [0, 0.01]]) - with local_archive(make_archive()) as archive: - renderer = MapRenderer(archive) - self.assertTrue(renderer.render(0, 0, 0, route)[1]) - self.assertFalse(renderer.render(0.01, 0, 0, route)[1]) # no road near the ego - with patch.object(archive, 'get', return_value=None): # a missing tile - image, valid = MapRenderer(archive).render(0, 0, 0, route) - self.assertFalse(valid) - self.assertEqual(image[128, 128], 128) # the route is still drawn - with local_archive(make_archive(field(3, field(1, b'buildings')))) as archive: # tiles without roads - self.assertFalse(MapRenderer(archive).render(0, 0, 0, route)[1]) - - -class TestRouteTracking(unittest.TestCase): - def test_snap_heading_and_window(self): - m = METER - tracker = RouteTracker(np.array([[0, 0], [0, 200 * m], [200 * m, 200 * m]])) # north 200m, then east 200m - longitude, latitude, bearing = tracker.snap(3 * m, 100 * m) # 3m right of the first leg - self.assertAlmostEqual(tracker.progress, 100, delta=0.5) - self.assertAlmostEqual(longitude, 0) # lateral error removed - self.assertAlmostEqual(latitude, 100 * m) - self.assertAlmostEqual(bearing, 0, places=3) - self.assertAlmostEqual(tracker.snap(0, 200 * m)[2], 45, places=2) # at the corner the +-50m chord is mid-turn - self.assertAlmostEqual(tracker.snap(0, 150 * m)[2], 45, places=2) # progress never decreases - tracker.snap(100 * m, 200 * m) # progress 300: the window ahead no longer includes the first leg - self.assertAlmostEqual(tracker.snap(150 * m, 200 * m)[0], 150 * m) # so the window's offset is applied - self.assertAlmostEqual(tracker.snap(200 * m, 200 * m)[2], 90, places=2) # the chord clips at the route's end - # Out and back along the same road: a fix near the far leg stays on the outbound leg within the window, - # while a fresh tracker searches anywhere and lands on the return leg. - route = np.array([[0, 0], [0, 1000 * m], [10 * m, 1000 * m], [10 * m, 0]]) - tracker = RouteTracker(route) - tracker.snap(1 * m, 100 * m) - self.assertAlmostEqual(tracker.snap(9 * m, 150 * m)[2], 0, places=3) - self.assertAlmostEqual(tracker.progress, 150, delta=0.5) - self.assertAlmostEqual(RouteTracker(route).snap(9 * m, 150 * m)[2], 180, places=3) - - -if __name__ == '__main__': - unittest.main() diff --git a/openpilot/selfdrive/navigation/tiles.py b/openpilot/selfdrive/navigation/tiles.py deleted file mode 100644 index e1b8414df..000000000 --- a/openpilot/selfdrive/navigation/tiles.py +++ /dev/null @@ -1,139 +0,0 @@ -"""Byte ranges of the hosted PMTiles basemap, cached on disk.""" -import re -import time -import urllib.parse -from pathlib import Path - -import requests - -from openpilot.common.hardware import PC -from openpilot.common.hardware.hw import Paths -from openpilot.common.params import Params -from openpilot.selfdrive.navigation.auth import nav_api_host, nav_auth -from openpilot.selfdrive.navigation.pmtiles import HEADER_LENGTH - -CACHE_LIMIT = 64 * 1024 * 1024 -MERGE_GAP = 256 * 1024 # tiles this close together in the archive are fetched in one range request - - -class RemoteTiles: - def __init__(self): - self.cache = Path(Paths.comma_home() if PC else '/data') / 'navigation' / 'tiles' - self.cache.mkdir(parents=True, exist_ok=True, mode=0o700) - self.session = requests.Session() - self.params = Params() - self.url = '' - self.expires = 0 - self.etag = None - self.size = 0 - version = self.cache / 'version' - try: - header = self._fetch(0, HEADER_LENGTH) # learns the archive's ETag and size - if not version.exists() or version.read_text() != f'{self.etag} {self.size}': - for cached in self.cache.iterdir(): # a new archive has new offsets - cached.unlink() - version.write_text(f'{self.etag} {self.size}') - self._store(0, header) - except Exception: - self.close() - raise - - def _request_with_retries(self, url: str, headers: dict, *, stream: bool = False): - last_response = None - for attempt in range(8): - try: - request_headers = {**headers, 'Connection': 'close'} - response = self.session.get(url, headers=request_headers, timeout=8, stream=stream) - if response.status_code not in (404, 408, 429) and response.status_code < 500: - return response - last_response = response - response.close() - except requests.RequestException: - if attempt == 7: - raise - if attempt < 7: - time.sleep(min(0.3 * (2 ** attempt), 2.0)) - assert last_response is not None - return last_response - - def _fetch(self, offset: int, length: int) -> bytes: - try: - if time.time() >= self.expires - 60: # noqa: TID251 - SAS expiry is a Unix timestamp - device_id, auth_headers = nav_auth(self.params) - device_path = urllib.parse.quote(device_id, safe='') - response = self._request_with_retries(f'{nav_api_host(self.params)}/v1/{device_path}/nav/tiles', auth_headers) - response.raise_for_status() - metadata = response.json() - self.url, self.expires = metadata['url'], metadata['expires'] - _, auth_headers = nav_auth(self.params) - headers = {**auth_headers, 'Range': f'bytes={offset}-{offset + length - 1}', 'Accept-Encoding': 'identity'} - if self.etag is not None: - headers['If-Match'] = self.etag - with self._request_with_retries(self.url, headers, stream=True) as response: - if response.status_code != 206: - raise OSError(f'Map tile range request failed ({response.status_code})') - match = re.fullmatch(r'bytes (\d+)-(\d+)/(\d+)', response.headers.get('Content-Range', '')) - if match is None or tuple(map(int, match.groups()[:2])) != (offset, offset + length - 1): - raise OSError('Invalid map tile range response') - size = int(match[3]) - etag = response.headers.get('ETag') - if not etag or (self.etag is not None and (etag != self.etag or size != self.size)): - raise OSError('Map archive changed during read') - data = bytearray() - for chunk in response.iter_content(chunk_size=min(length + 1, 65536)): - data.extend(chunk) - if len(data) > length: - raise OSError('Oversized map tile range response') - if len(data) != length: - raise OSError('Truncated map tile range response') - self.size, self.etag = size, etag - return bytes(data) - except (requests.RequestException, ValueError, KeyError, TypeError, RuntimeError): - # Requests exceptions may contain the URL or authorization headers; keep them out of logs. - raise OSError('Unable to fetch map tiles') from None - - def _store(self, offset: int, data: bytes): - (self.cache / f'{offset}-{len(data)}').write_bytes(data) - - def _trim(self): - files = [(cached.stat().st_mtime, cached.stat().st_size, cached) for cached in self.cache.iterdir() if re.fullmatch(r'\d+-\d+', cached.name)] - size = sum(cached_size for _, cached_size, _ in files) - for _, cached_size, cached in sorted(files): - if size <= CACHE_LIMIT: - break - cached.unlink(missing_ok=True) - size -= cached_size - - def read(self, offset: int, length: int) -> bytes: - try: - data = (self.cache / f'{offset}-{length}').read_bytes() - if len(data) == length: - return data - except FileNotFoundError: - pass - data = self._fetch(offset, length) - self._store(offset, data) - self._trim() - return data - - def prefetch(self, ranges): - """Cache (offset, length) ranges ahead of reads, fetching neighboring ones in a single request.""" - missing = sorted((offset, length) for offset, length in set(ranges) if not (self.cache / f'{offset}-{length}').exists()) - if not missing: - return - groups = [[missing[0]]] - for offset, length in missing[1:]: - last_offset, last_length = groups[-1][-1] - if offset - (last_offset + last_length) <= MERGE_GAP: - groups[-1].append((offset, length)) - else: - groups.append([(offset, length)]) - for group in groups: - start, end = group[0][0], group[-1][0] + group[-1][1] - data = self._fetch(start, end - start) - for offset, length in group: - self._store(offset, data[offset - start:offset - start + length]) - self._trim() - - def close(self): - self.session.close() diff --git a/openpilot/selfdrive/phone_navi/agent.py b/openpilot/selfdrive/phone_navi/agent.py index 1c841cf80..c80255acf 100644 --- a/openpilot/selfdrive/phone_navi/agent.py +++ b/openpilot/selfdrive/phone_navi/agent.py @@ -4,9 +4,11 @@ from __future__ import annotations import base64 import hashlib +import io import json import logging import os +import queue import socket import struct import threading @@ -14,12 +16,22 @@ import time from pathlib import Path from typing import Any +import numpy as np +from PIL import Image, ImageOps +from msgq.visionipc import VisionIpcServer + import openpilot.cereal.messaging as messaging +from openpilot.cereal.visionipc import VisionStreamType from openpilot.common.params import Params +from openpilot.selfdrive.navigation.helpers import latest_gps_source LOGGER = logging.getLogger("phone_navi") DISCOVERY_PORT = 7705 WEBSOCKET_PORT = 7714 +MAP_STREAM_NAME = "phone_navi" +MAP_WIDTH = 384 +MAP_HEIGHT = 512 +MAP_JPEG_MAX_BYTES = 4 * 1024 * 1024 SERVICE = "mrone-phone-navi" STATE_PATH = Path("/data/phone_navi_state.json") MAX_FRAME_BYTES = 16 * 1024 * 1024 @@ -78,6 +90,56 @@ def _send_json(conn: socket.socket, payload: dict[str, Any]) -> None: _send_frame(conn, 0x1, json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8")) +class MapStreamPublisher: + """Decode phone JPEG map frames and expose RGB frames on VisionIPC.""" + + def __init__(self): + self.server = VisionIpcServer(MAP_STREAM_NAME) + self.server.create_buffers_with_sizes( + VisionStreamType.VISION_STREAM_MAP, + 4, + MAP_WIDTH, + MAP_HEIGHT, + MAP_WIDTH * MAP_HEIGHT * 3, + MAP_WIDTH * 3, + 0, + ) + self.server.start_listener() + self.frames: queue.Queue[bytes | None] = queue.Queue(maxsize=2) + self.frame_id = 0 + threading.Thread(target=self._run, name="phone-map-stream", daemon=True).start() + + def submit(self, jpeg: bytes) -> None: + if not jpeg or len(jpeg) > MAP_JPEG_MAX_BYTES: + return + try: + self.frames.put_nowait(jpeg) + except queue.Full: + try: + self.frames.get_nowait() + except queue.Empty: + pass + try: + self.frames.put_nowait(jpeg) + except queue.Full: + pass + + def _run(self) -> None: + while True: + jpeg = self.frames.get() + if jpeg is None: + return + try: + with Image.open(io.BytesIO(jpeg)) as source: + image = ImageOps.fit(source.convert("RGB"), (MAP_WIDTH, MAP_HEIGHT), method=Image.Resampling.BILINEAR) + rgb = np.asarray(image, dtype=np.uint8).reshape(-1) + now = time.monotonic_ns() + self.server.send(VisionStreamType.VISION_STREAM_MAP, rgb.tobytes(), self.frame_id, now, now) + self.frame_id += 1 + except Exception: + LOGGER.exception("phone map frame decode failed") + + def _handshake(conn: socket.socket, request: bytes) -> str | None: lines = request.decode("latin-1", "replace").split("\r\n") if not lines or not lines[0].startswith("GET "): @@ -108,6 +170,8 @@ class CerealPublisher: self.lock = threading.Lock() self.last_route: list[dict[str, float]] | None = None self.last_instruction: dict[str, Any] | None = None + self.heartbeat_stop = threading.Event() + threading.Thread(target=self._heartbeat, name="phone-nav-heartbeat", daemon=True).start() def publish_route(self, coordinates: list[dict[str, Any]]) -> None: cleaned = [] @@ -153,11 +217,38 @@ class CerealPublisher: self.pm.send("navInstruction", message) self.last_instruction = payload - def resend_last(self, conn: socket.socket) -> None: + def replay(self) -> None: if self.last_route is not None: - _send_json(conn, {"type": "navRouteAck", "count": len(self.last_route)}) + self.publish_route(self.last_route) if self.last_instruction is not None: - _send_json(conn, {"type": "navInstructionAck"}) + self.publish_instruction(self.last_instruction) + + def clear(self) -> None: + with self.lock: + route = messaging.new_message("navRoute", valid=True) + route.navRoute.coordinates = [] + self.pm.send("navRoute", route) + instruction = messaging.new_message("navInstruction", valid=False) + self.pm.send("navInstruction", instruction) + self.last_route = None + self.last_instruction = None + + def _heartbeat(self) -> None: + last_route_at = 0.0 + while not self.heartbeat_stop.is_set(): + try: + if self.last_instruction is not None: + self.publish_instruction(self.last_instruction) + now = time.monotonic() + if self.last_route is not None and now - last_route_at >= 5.0: + self.publish_route(self.last_route) + last_route_at = now + except Exception: + LOGGER.exception("phone navigation heartbeat failed") + self.heartbeat_stop.wait(1.0) + + def resend_last(self) -> None: + self.replay() class PhoneNaviServer: @@ -166,9 +257,51 @@ class PhoneNaviServer: self.device_id = str(self.params.get("DongleId") or "comma-device") self.hostname = socket.gethostname() self.publisher = CerealPublisher() + self.map_publisher = MapStreamPublisher() self.stop_event = threading.Event() + self.clients_lock = threading.Lock() + self.clients: dict[socket.socket, threading.Lock] = {} + self.last_location: dict[str, Any] | None = None + self.state = self.load_state() Path("/data/phone_navi_active").touch() self.params.put_bool("NavigationEnabled", True, block=True) + self.restore_state() + threading.Thread(target=self.telemetry_loop, name="phone-nav-telemetry", daemon=True).start() + + def send_json(self, conn: socket.socket, payload: dict[str, Any]) -> None: + with self.clients_lock: + send_lock = self.clients.get(conn) + if send_lock is None: + return + with send_lock: + _send_json(conn, payload) + + def telemetry_loop(self) -> None: + sm = messaging.SubMaster(["gpsLocation", "gpsLocationExternal"]) + while not self.stop_event.is_set(): + sm.update(200) + source = latest_gps_source(sm) + if source is None: + continue + gps = sm[source] + location = { + "type": "vehicleLocation", + "latitude": float(gps.latitude), + "longitude": float(gps.longitude), + "bearingDeg": float(gps.bearingDeg), + "speed": float(gps.speed), + "horizontalAccuracy": float(gps.horizontalAccuracy), + "satelliteCount": int(gps.satelliteCount), + "source": source, + } + self.last_location = location + with self.clients_lock: + clients = list(self.clients.items()) + for conn, _ in clients: + try: + self.send_json(conn, location) + except OSError: + pass def discovery_loop(self) -> None: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) @@ -207,15 +340,76 @@ class PhoneNaviServer: except OSError: pass + def load_state(self) -> dict[str, dict[str, Any] | None]: + state: dict[str, dict[str, Any] | None] = {"destination": None, "route": None, "instruction": None} + try: + saved = json.loads(STATE_PATH.read_text(encoding="utf-8")) + for key in state: + if isinstance(saved.get(key), dict): + state[key] = saved[key] + legacy = saved.get("last_message") + if isinstance(legacy, dict): + legacy_type = legacy.get("type") + if legacy_type == "destination" and state["destination"] is None: + state["destination"] = legacy + elif legacy_type == "navRoute" and state["route"] is None: + state["route"] = legacy + elif legacy_type == "navInstruction" and state["instruction"] is None: + state["instruction"] = legacy + except (OSError, ValueError, TypeError): + pass + return state + def save_state(self, payload: dict[str, Any]) -> None: - state = {"updated_at": time.time(), "last_message": payload} + message_type = payload.get("type") + if message_type == "destination": + self.state["destination"] = payload + elif message_type == "navRoute": + self.state["route"] = payload + elif message_type == "navInstruction": + self.state["instruction"] = payload try: tmp = STATE_PATH.with_suffix(".tmp") - tmp.write_text(json.dumps(state, ensure_ascii=False, separators=(",", ":")), encoding="utf-8") + data = {"updated_at": time.time(), **self.state} + tmp.write_text(json.dumps(data, ensure_ascii=False, separators=(",", ":")), encoding="utf-8") os.replace(tmp, STATE_PATH) except OSError: LOGGER.exception("failed to save phone navigation state") + def clear_navigation(self) -> None: + self.publisher.clear() + self.state = {"destination": None, "route": None, "instruction": None} + try: + tmp = STATE_PATH.with_suffix(".tmp") + data = {"updated_at": time.time(), **self.state} + tmp.write_text(json.dumps(data, ensure_ascii=False, separators=(",", ":")), encoding="utf-8") + os.replace(tmp, STATE_PATH) + except OSError: + LOGGER.exception("failed to clear phone navigation state") + self.params.remove("NavDestination") + self.params.put_bool("NavigationEnabled", False, block=True) + + def restore_state(self) -> None: + destination = self.state.get("destination") + if isinstance(destination, dict): + longitude = float(destination.get("longitude") or 0.0) + latitude = float(destination.get("latitude") or 0.0) + if longitude != 0.0 and latitude != 0.0: + self.params.put("NavDestination", { + "name": str(destination.get("name") or ""), + "longitude": longitude, + "latitude": latitude, + }, block=True) + route = self.state.get("route") + if isinstance(route, dict) and isinstance(route.get("coordinates"), list): + try: + self.publisher.publish_route(route["coordinates"]) + except ValueError: + pass + instruction = self.state.get("instruction") + if isinstance(instruction, dict): + self.publisher.publish_instruction(instruction) + def handle_client(self, conn: socket.socket, address: tuple[str, int]) -> None: try: conn.settimeout(10) @@ -229,9 +423,18 @@ class PhoneNaviServer: if path is None or not path.startswith("/api/phone-navi/v1"): return conn.settimeout(None) + with self.clients_lock: + self.clients[conn] = threading.Lock() LOGGER.warning("phone_navi client connected: %s", address) - _send_json(conn, {"type": "welcome", "protocol": 1, "device_id": self.device_id}) - self.publisher.resend_last(conn) + map_info = {"encoding": "jpeg", "width": MAP_WIDTH, "height": MAP_HEIGHT, "fps": 5} + self.send_json(conn, {"type": "welcome", "protocol": 1, "device_id": self.device_id, "map": map_info}) + self.publisher.resend_last() + if self.publisher.last_route is not None: + self.send_json(conn, {"type": "navRouteAck", "count": len(self.publisher.last_route)}) + if self.publisher.last_instruction is not None: + self.send_json(conn, {"type": "navInstructionAck"}) + if self.last_location is not None: + self.send_json(conn, self.last_location) while not self.stop_event.is_set(): opcode, data = _read_frame(conn) if opcode == 0x8: @@ -239,6 +442,9 @@ class PhoneNaviServer: if opcode == 0x9: _send_frame(conn, 0xA, data) continue + if opcode == 0x2: + self.map_publisher.submit(data) + continue if opcode != 0x1: continue payload = _safe_json(data) @@ -247,7 +453,7 @@ class PhoneNaviServer: message_type = str(payload.get("type") or "") self.save_state(payload) if message_type == "hello": - _send_json(conn, {"type": "helloAck", "protocol": 1}) + self.send_json(conn, {"type": "helloAck", "protocol": 1, "map": map_info}) elif message_type == "destination": destination = { "name": str(payload.get("name") or ""), @@ -257,19 +463,24 @@ class PhoneNaviServer: if destination["longitude"] != 0.0 and destination["latitude"] != 0.0: self.params.put("NavDestination", destination, block=True) self.params.put_bool("NavigationEnabled", True, block=True) - _send_json(conn, {"type": "destinationAck"}) + self.send_json(conn, {"type": "destinationAck"}) elif message_type == "navRoute": coordinates = payload.get("coordinates") or [] self.publisher.publish_route(coordinates) - _send_json(conn, {"type": "navRouteAck", "count": len(coordinates)}) + self.send_json(conn, {"type": "navRouteAck", "count": len(coordinates)}) elif message_type == "navInstruction": self.publisher.publish_instruction(payload) - _send_json(conn, {"type": "navInstructionAck"}) + self.send_json(conn, {"type": "navInstructionAck"}) + elif message_type == "navigationStop": + self.clear_navigation() + self.send_json(conn, {"type": "navigationStopAck"}) else: - _send_json(conn, {"type": "error", "message": f"unsupported type: {message_type}"}) + self.send_json(conn, {"type": "error", "message": f"unsupported type: {message_type}"}) except Exception as exc: LOGGER.warning("phone_navi client disconnected: %s", exc) finally: + with self.clients_lock: + self.clients.pop(conn, None) try: conn.close() except OSError: diff --git a/openpilot/selfdrive/ui/onroad/navigation.py b/openpilot/selfdrive/ui/onroad/navigation.py index ea3a83d5b..8f62e58b3 100644 --- a/openpilot/selfdrive/ui/onroad/navigation.py +++ b/openpilot/selfdrive/ui/onroad/navigation.py @@ -4,8 +4,6 @@ import pyray as rl from msgq.visionipc import VisionIpcClient from openpilot.cereal.visionipc import VisionStreamType from openpilot.common.constants import CV -from openpilot.selfdrive.navigation.helpers import latest_gps_source -from openpilot.selfdrive.navigation.renderer import EGO, ROUTE_INTENSITY, SIZE from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import FontWeight, TextAlignmentVertical from openpilot.system.ui.widgets import Widget @@ -14,12 +12,9 @@ from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label PANEL_WIDTH = 261 CONNECTION_RETRY_INTERVAL = 1.0 STALE_SECONDS = 3 -MARKER_MARGIN = 18 # pixels kept below the ego so its marker stays visible - -# Recolor the model's grayscale frame for people: dim the white roads and make the grey route pop. Only the route's -# exact level (and its neighbors) is recolored, so antialiased road edges passing through that grey stay grey. -MAP_COLORS = np.repeat(np.round(np.arange(256) * 0.45).astype(np.uint8)[:, None], 3, axis=1) -MAP_COLORS[ROUTE_INTENSITY - 1:ROUTE_INTENSITY + 2] = (72, 235, 132) +PHONE_MAP_STREAM_NAME = 'phone_navi' +FALLBACK_MAP_WIDTH = 384 +FALLBACK_MAP_HEIGHT = 512 def format_distance(meters: float) -> str: @@ -30,14 +25,16 @@ def format_distance(meters: float) -> str: class NavigationPanel(Widget): - """Turn instructions around the map frames mapsd publishes for the model.""" + """Turn instructions around RGB map frames supplied by the phone navigation app.""" def __init__(self): super().__init__() - self._client = VisionIpcClient('mapsd', VisionStreamType.VISION_STREAM_MAP, True) + self._client = VisionIpcClient(PHONE_MAP_STREAM_NAME, VisionStreamType.VISION_STREAM_MAP, True) self._last_connect = 0.0 self._frame_time = 0.0 self._texture = None + self._map_width = FALLBACK_MAP_WIDTH + self._map_height = FALLBACK_MAP_HEIGHT self._eta_label = UnifiedLabel('', 34, FontWeight.BOLD, rl.WHITE, scroll=True) self._accent = rl.Color(155, 230, 185, 255) self._muted = rl.Color(162, 175, 185, 255) @@ -50,13 +47,18 @@ class NavigationPanel(Widget): if not self._client.connect(False) or not self._client.num_buffers: return self._unload_texture() - self._texture = rl.load_texture_from_image(rl.Image(None, SIZE, SIZE, 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_R8G8B8)) + self._map_width = int(self._client.width or FALLBACK_MAP_WIDTH) + self._map_height = int(self._client.height or FALLBACK_MAP_HEIGHT) + self._texture = rl.load_texture_from_image( + rl.Image(None, self._map_width, self._map_height, 1, rl.PixelFormat.PIXELFORMAT_UNCOMPRESSED_R8G8B8) + ) frame = self._client.recv(timeout_ms=0) - if frame is not None: - gray = np.frombuffer(frame.data, dtype=np.uint8, count=SIZE * SIZE) - rl.update_texture(self._texture, rl.ffi.cast("void *", rl.ffi.from_buffer(MAP_COLORS[gray]))) - # Without a usable map, mapsd publishes black frames with a zero timestamp. - self._frame_time = rl.get_time() if self._client.timestamp_sof else 0.0 + if frame is not None and self._texture is not None: + count = self._map_width * self._map_height * 3 + if len(frame.data) >= count: + rgb = np.frombuffer(frame.data, dtype=np.uint8, count=count).reshape((self._map_height, self._map_width, 3)) + rl.update_texture(self._texture, rl.ffi.cast("void *", rl.ffi.from_buffer(rgb))) + self._frame_time = rl.get_time() if self._client.timestamp_sof else 0.0 @property def routing(self): @@ -67,9 +69,9 @@ class NavigationPanel(Widget): return self._texture is not None and 0 <= rl.get_time() - self._frame_time < STALE_SECONDS def _draw_status(self, rect): - """What the map is waiting on, in place of the map.""" + """What the phone map is waiting on, in place of the map.""" statuses = ( - ('GPS fix', latest_gps_source(ui_state.sm) is not None), + ('Phone stream', self._client.is_connected()), ('Route', self.routing), ('Map frames', self.map_ready), ) @@ -102,10 +104,11 @@ class NavigationPanel(Widget): map_rect = rl.Rectangle(x+8, y+61, rect.width-16, rect.height-111) rl.draw_rectangle_rec(map_rect, rl.BLACK) if self.map_ready: - # A 1:1 crop of the model's frame, centered horizontally with the ego near the bottom. - width, height = min(map_rect.width, SIZE), min(map_rect.height, SIZE) - source = rl.Rectangle((SIZE - width) // 2, min(SIZE - height, max(0, EGO[1] + MARKER_MARGIN - height)), width, height) - target = rl.Rectangle(map_rect.x + (map_rect.width - width) / 2, map_rect.y + map_rect.height - height, width, height) + scale = min(map_rect.width / self._map_width, map_rect.height / self._map_height) + width = self._map_width * scale + height = self._map_height * scale + source = rl.Rectangle(0, 0, self._map_width, self._map_height) + target = rl.Rectangle(map_rect.x + (map_rect.width - width) / 2, map_rect.y + (map_rect.height - height) / 2, width, height) rl.draw_texture_pro(self._texture, source, target, rl.Vector2(0, 0), 0, rl.WHITE) else: self._draw_status(map_rect) @@ -121,4 +124,4 @@ class NavigationPanel(Widget): self._texture = None def close(self): - self._unload_texture() + self._unload_texture() \ No newline at end of file diff --git a/openpilot/system/manager/process.py b/openpilot/system/manager/process.py index 363f7f38e..74bb3a400 100644 --- a/openpilot/system/manager/process.py +++ b/openpilot/system/manager/process.py @@ -103,6 +103,17 @@ class ManagerProcess(ABC): return ret + def prepare_start(self) -> bool: + if self.proc is None: + return True + if self.proc.is_alive(): + return False + exit_code = self.proc.exitcode + self.proc.join() + self.proc = None + cloudlog.warning(f"{self.name} exited with {exit_code}, restarting") + return True + def signal(self, sig: int) -> None: if self.proc is None: return @@ -144,7 +155,7 @@ class NativeProcess(ManagerProcess): if self.shutting_down: self.stop() - if self.proc is not None: + if not self.prepare_start(): return cwd = os.path.join(BASEDIR, self.cwd) @@ -168,7 +179,7 @@ class PythonProcess(ManagerProcess): if self.shutting_down: self.stop() - if self.proc is not None: + if not self.prepare_start(): return cloudlog.info(f"starting python {self.module}")