mawei/openpilot
1
0
Fork 0
mirror of https://jihulab.com/mr-one/openpilot.git synced 2026-10-26 20:43:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigation: stream phone maps and remove NAS gateway

Browse Source
This commit is contained in:
1okko
2026-10-22 23:08:48 +08:00
parent e61928652d
commit df4fa9719f
8 changed files with 262 additions and 1095 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/navigation/auth.py
-43
View File
@@ -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}
openpilot/selfdrive/navigation/mapsd.py
-83
View File
@@ -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()
openpilot/selfdrive/navigation/navd.py
-236
View File
@@ -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()
openpilot/selfdrive/navigation/tests/test_navigation.py
-557
View File
@@ -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()
openpilot/selfdrive/navigation/tiles.py
-139
View File
@@ -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()
openpilot/selfdrive/phone_navi/agent.py
+223 -12
View File
@@ -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:
openpilot/selfdrive/ui/onroad/navigation.py
+26 -23
View File
@@ -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()
openpilot/system/manager/process.py
+13 -2
View File
@@ -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}")