mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
dirwin31
971 lines
44 KiB
Python
971 lines
44 KiB
Python
"""Heading-up road-line map, drawn as a translucent overlay on the Android Auto screen.
|
|
|
|
Roads come from the compact tiles ``navtilesd`` writes; this module never
|
|
touches the network. The work is split so no frame waits on anything slow:
|
|
|
|
worker thread reads and decodes tiles, and turns roads and the route into
|
|
triangle strips for a north-up canvas around the car
|
|
prepare() outside any parent render target: draws a finished canvas
|
|
into a texture (only when one is ready), then composes the
|
|
widget: canvas rotated heading-up, the car, a glass tint
|
|
draw() inside the frame: one textured quad through a shader that
|
|
rounds and feathers the edges; opacity fades the map, not navigation guidance
|
|
|
|
Colors are premultiplied end to end, so translucent glows and the feathered
|
|
edge composite without dark fringes.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import threading
|
|
import time
|
|
from collections import OrderedDict
|
|
from collections.abc import Callable, Sequence
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
import pyray as rl
|
|
|
|
from openpilot.starpilot.gps.acquisition import GpsAcquisition, acquisition_progress
|
|
from openpilot.starpilot.gps.source import GPS_MAX_AGE_NS, PHONE_MAX_AGE_NS
|
|
from openpilot.starpilot.navigation.road_tiles import (
|
|
DATA_ZOOM, EXTENT, LINK, MAJOR, MINOR, MOTORWAY, STREET, RoadTile, TileFormatError, TileKey, decode_road_tile,
|
|
encode_road_tile, meters_per_tile, world_xy,
|
|
)
|
|
from openpilot.system.ui.lib.application import GL_VERSION
|
|
|
|
SUPERSAMPLE = 1.5 # canvas pixels per screen pixel: edges are smoothed when it is scaled down
|
|
MAX_CANVAS = 2560
|
|
TILE_CACHE = 48
|
|
MISSING_RETRY_S = 3.0
|
|
DEAD_RECKON_S = 1.0
|
|
PHONE_DEAD_RECKON_S = GPS_MAX_AGE_NS / 1e9 # cover phone measurement/transport gaps, independently of the 10 s hold
|
|
ROUTE_STALE_OVERLAY_SECONDS = 10.0 # a route lost mid-guidance only brings back "Starting navigation" after this long
|
|
STARTING_OVERLAY_MIN_SECONDS = 5.0 # "Starting navigation" stays up at least this long, even when the route loads sooner
|
|
DESTINATION_MAX_CHARS = 32
|
|
CAR_ANCHOR = 0.68 # the car sits below center so more road ahead is visible
|
|
ZOOM_SLOW_M_PER_PX = 0.9 # parking-lot and city detail
|
|
ZOOM_FAST_M_PER_PX = 3.4 # highway overview
|
|
MIN_HEADING_SPEED = 1.5 # m/s; GPS heading is noise below this
|
|
GUIDANCE_SLACK_PX = 4.0 # screen pixels of driven route left under the puck before guidance is trimmed again
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RoadStyle:
|
|
fill: tuple[int, int, int]
|
|
width: float # screen pixels at the reference zoom
|
|
hide_above_m_per_px: float = math.inf
|
|
|
|
|
|
# Bottom to top. Casings go under every fill so crossings read cleanly.
|
|
ROAD_STYLES: dict[int, RoadStyle] = {
|
|
MINOR: RoadStyle((112, 124, 140), 2.4, hide_above_m_per_px=2.2),
|
|
STREET: RoadStyle((176, 188, 204), 3.4, hide_above_m_per_px=4.5),
|
|
LINK: RoadStyle((214, 220, 232), 3.6),
|
|
MAJOR: RoadStyle((236, 240, 246), 5.2),
|
|
MOTORWAY: RoadStyle((255, 196, 92), 6.8),
|
|
}
|
|
CASING = (0, 0, 0, 200)
|
|
CASING_EXTRA = 2.6
|
|
ROUTE_GLOW = (150, 110, 255, 90)
|
|
ROUTE_CORE = (178, 150, 255, 255)
|
|
ROUTE_DONE = (118, 104, 150, 200)
|
|
GLASS = (8, 12, 20, 132)
|
|
SOURCE_LABELS = {"comma": "Comma GPS", "car": "Car GPS", "phone": "Phone GPS"}
|
|
REFERENCE_M_PER_PX = 1.4
|
|
|
|
|
|
def premultiplied(color: Sequence[int], alpha_scale: float = 1.0) -> rl.Color:
|
|
r, g, b, a = (*color, 255) if len(color) == 3 else color
|
|
alpha = a / 255.0 * alpha_scale
|
|
return rl.Color(round(r * alpha), round(g * alpha), round(b * alpha), round(255 * alpha))
|
|
|
|
|
|
def zoom_for_speed(speed_mps: float) -> float:
|
|
"""Meters per screen pixel: street detail when slow, a wider view at highway speed."""
|
|
t = min(1.0, max(0.0, (speed_mps - 8.0) / 24.0))
|
|
t = t * t * (3 - 2 * t)
|
|
return ZOOM_SLOW_M_PER_PX + (ZOOM_FAST_M_PER_PX - ZOOM_SLOW_M_PER_PX) * t
|
|
|
|
|
|
# ---------------------------------------------------------------- geometry
|
|
|
|
def line_strip(points: np.ndarray, offsets: np.ndarray, half_width: float, bounds: tuple[float, float, float, float],
|
|
max_miter: float = 2.5) -> np.ndarray:
|
|
"""One triangle strip for many polylines: mitered quads, joined by degenerate triangles.
|
|
|
|
``points`` (N, 2) and ``offsets`` (L + 1) describe L polylines of at least two
|
|
points. Lines entirely outside ``bounds`` (x0, y0, x1, y1) are skipped.
|
|
"""
|
|
if len(offsets) < 2 or len(points) < 2:
|
|
return np.empty((0, 2), np.float32)
|
|
starts, ends = offsets[:-1], offsets[1:]
|
|
x0, y0, x1, y1 = bounds
|
|
lo, hi = np.minimum.reduceat(points, starts, axis=0), np.maximum.reduceat(points, starts, axis=0)
|
|
keep = (hi[:, 0] >= x0) & (lo[:, 0] <= x1) & (hi[:, 1] >= y0) & (lo[:, 1] <= y1) & (ends - starts >= 2)
|
|
if not keep.any():
|
|
return np.empty((0, 2), np.float32)
|
|
lengths = (ends - starts)[keep]
|
|
line_offsets = np.concatenate(([0], np.cumsum(lengths)[:-1]))
|
|
index = np.arange(int(lengths.sum())) - np.repeat(line_offsets, lengths) + np.repeat(starts[keep], lengths)
|
|
p = points[index].astype(np.float32)
|
|
first = np.zeros(len(p), bool)
|
|
first[np.concatenate(([0], np.cumsum(lengths)[:-1]))] = True
|
|
last = np.roll(first, -1)
|
|
last[-1] = True
|
|
|
|
seg = np.diff(p, axis=0)
|
|
norm = np.hypot(seg[:, 0], seg[:, 1])
|
|
seg /= np.maximum(norm, 1e-6)[:, None]
|
|
normal = np.stack((-seg[:, 1], seg[:, 0]), axis=1) # left of travel
|
|
n_prev = np.vstack((normal[:1], normal)) # segment ending at each vertex
|
|
n_next = np.vstack((normal, normal[-1:])) # segment starting at each vertex
|
|
n_prev[first] = n_next[first]
|
|
n_next[last] = n_prev[last]
|
|
miter = n_prev + n_next
|
|
miter_len = np.hypot(miter[:, 0], miter[:, 1])
|
|
flat = miter_len < 1e-3 # a full reversal: fall back to the segment normal
|
|
miter[flat] = n_next[flat]
|
|
miter_len[flat] = 1.0
|
|
miter /= miter_len[:, None]
|
|
scale = 1.0 / np.clip(np.einsum("ij,ij->i", miter, n_next), 1.0 / max_miter, 1.0)
|
|
offset = miter * (scale * half_width)[:, None]
|
|
|
|
# Right of travel, then left: front-facing for raylib in y-down screen space,
|
|
# so backface culling never drops a road.
|
|
strip = np.empty((len(p) * 2, 2), np.float32)
|
|
strip[0::2] = p - offset
|
|
strip[1::2] = p + offset
|
|
# Repeat the last vertex of each line and the first of the next: two zero-area
|
|
# triangles bridge the gap and keep every real triangle's winding.
|
|
repeat = np.ones(len(strip), np.int64)
|
|
line_starts = np.flatnonzero(first) * 2
|
|
repeat[line_starts[1:]] += 1
|
|
repeat[line_starts[1:] - 1] += 1
|
|
return np.repeat(strip, repeat, axis=0)
|
|
|
|
|
|
@dataclass
|
|
class CanvasRequest:
|
|
center: tuple[float, float] # world (data-zoom tile units) at the canvas center
|
|
m_per_px: float # screen meters per pixel this canvas was built for
|
|
side: int # canvas texture side in pixels (supersampled)
|
|
route: np.ndarray | None # (N, 2) world points ahead of the car
|
|
route_done: np.ndarray | None # (N, 2) world points already driven
|
|
tiles: tuple[TileKey, ...]
|
|
|
|
|
|
@dataclass
|
|
class CanvasGeometry:
|
|
request: CanvasRequest
|
|
layers: list[tuple[rl.Color, np.ndarray]] = field(default_factory=list)
|
|
tiles_missing: int = 0
|
|
guidance: list[tuple[rl.Color, np.ndarray]] = field(default_factory=list)
|
|
|
|
|
|
def build_canvas(request: CanvasRequest, tiles: dict[TileKey, RoadTile], latitude: float) -> CanvasGeometry:
|
|
"""Screen-space strips for a north-up canvas. Pure numpy; runs off the render thread."""
|
|
side = request.side
|
|
px_per_m = SUPERSAMPLE / request.m_per_px
|
|
px_per_unit = meters_per_tile(latitude) * px_per_m # canvas pixels per world tile unit
|
|
cx, cy = request.center
|
|
bounds = (-8.0, -8.0, side + 8.0, side + 8.0)
|
|
ratio = math.sqrt(REFERENCE_M_PER_PX / request.m_per_px)
|
|
width_scale = SUPERSAMPLE * min(1.35, max(0.7, ratio))
|
|
geometry = CanvasGeometry(request)
|
|
per_class: dict[int, list[tuple[np.ndarray, np.ndarray]]] = {}
|
|
for key in request.tiles:
|
|
tile = tiles.get(key)
|
|
if tile is None:
|
|
geometry.tiles_missing += 1
|
|
continue
|
|
ox = (key.x - cx) * px_per_unit + side / 2
|
|
oy = (key.y - cy) * px_per_unit + side / 2
|
|
scale = px_per_unit / EXTENT
|
|
for road_class, (points, offsets) in tile.lines.items():
|
|
if request.m_per_px > ROAD_STYLES[road_class].hide_above_m_per_px:
|
|
continue
|
|
per_class.setdefault(road_class, []).append((points * scale + (ox, oy), offsets))
|
|
merged = {}
|
|
for road_class, parts in per_class.items():
|
|
points = np.concatenate([part[0] for part in parts])
|
|
offsets, base = [np.zeros(1, np.int32)], 0
|
|
for part_points, part_offsets in parts:
|
|
offsets.append(part_offsets[1:] + base)
|
|
base += len(part_points)
|
|
merged[road_class] = (points, np.concatenate(offsets))
|
|
order = [c for c in ROAD_STYLES if c in merged]
|
|
for road_class in order:
|
|
style = ROAD_STYLES[road_class]
|
|
strip = line_strip(*merged[road_class], (style.width + CASING_EXTRA) * width_scale / 2, bounds)
|
|
if len(strip):
|
|
geometry.layers.append((premultiplied(CASING), strip))
|
|
for road_class in order:
|
|
style = ROAD_STYLES[road_class]
|
|
strip = line_strip(*merged[road_class], style.width * width_scale / 2, bounds)
|
|
if len(strip):
|
|
geometry.layers.append((premultiplied(style.fill), strip))
|
|
|
|
geometry.guidance = route_guidance(request.route, request, latitude)
|
|
return geometry
|
|
|
|
|
|
def route_guidance(route, request: CanvasRequest, latitude: float):
|
|
"""Build only the remaining route; the road canvas never bakes in a driven tail."""
|
|
if route is None or len(route) < 2:
|
|
return []
|
|
scale = meters_per_tile(latitude) * SUPERSAMPLE / request.m_per_px
|
|
points = ((route - request.center) * scale + request.side / 2).astype(np.float32)
|
|
width_scale = SUPERSAMPLE * min(1.35, max(0.7, math.sqrt(REFERENCE_M_PER_PX / request.m_per_px)))
|
|
bounds = (-8.0, -8.0, request.side + 8.0, request.side + 8.0)
|
|
# Every frame draws these strips: keep only the stretches that can reach the canvas, widest line included.
|
|
reach = 8.0 + 17.0 * width_scale / 2
|
|
points, offsets = visible_runs(points, (-reach, -reach, request.side + reach, request.side + reach))
|
|
layers = []
|
|
for color, width in ((ROUTE_GLOW, 17.0), (ROUTE_CORE, 7.0)):
|
|
strip = line_strip(points, offsets, width * width_scale / 2, bounds)
|
|
if len(strip):
|
|
layers.append((premultiplied(color), strip))
|
|
return layers
|
|
|
|
|
|
def visible_runs(points: np.ndarray, bounds: tuple[float, float, float, float]) -> tuple[np.ndarray, np.ndarray]:
|
|
"""The polyline's stretches whose segments can touch ``bounds``, as (points, offsets) for line_strip.
|
|
|
|
A stretch is broken only where a whole segment lies outside, so seams never show inside the bounds.
|
|
"""
|
|
x0, y0, x1, y1 = bounds
|
|
a, b = points[:-1], points[1:]
|
|
lo, hi = np.minimum(a, b), np.maximum(a, b)
|
|
keep = (hi[:, 0] >= x0) & (lo[:, 0] <= x1) & (hi[:, 1] >= y0) & (lo[:, 1] <= y1)
|
|
if keep.all():
|
|
return points, np.array([0, len(points)], np.int32)
|
|
edges = np.diff(np.concatenate(([0], keep.astype(np.int8), [0])))
|
|
runs = [points[start:end + 1] for start, end in zip(np.flatnonzero(edges == 1), np.flatnonzero(edges == -1), strict=True)]
|
|
if not runs:
|
|
return points[:0], np.zeros(1, np.int32)
|
|
return np.concatenate(runs), np.concatenate(([0], np.cumsum([len(run) for run in runs]))).astype(np.int32)
|
|
|
|
|
|
def split_route(route: np.ndarray | None, position: tuple[float, float]) -> tuple[np.ndarray | None, np.ndarray | None]:
|
|
"""(driven, ahead) world polylines, split at the point on the route nearest the car."""
|
|
if route is None or len(route) < 2:
|
|
return None, None
|
|
a, b = route[:-1], route[1:]
|
|
ab = b - a
|
|
t = np.clip(np.einsum("ij,ij->i", np.asarray(position) - a, ab) / np.maximum(np.einsum("ij,ij->i", ab, ab), 1e-12), 0, 1)
|
|
nearest = a + ab * t[:, None]
|
|
index = int(np.argmin(np.hypot(*(nearest - position).T)))
|
|
split = nearest[index]
|
|
return np.vstack((route[:index + 1], split)), np.vstack((split, route[index + 1:]))
|
|
|
|
|
|
# ---------------------------------------------------------------- data
|
|
|
|
@dataclass(frozen=True)
|
|
class MapFix:
|
|
latitude: float
|
|
longitude: float
|
|
bearing: float | None
|
|
speed: float
|
|
monotonic: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class MapInput:
|
|
fix: MapFix | None
|
|
route: tuple[tuple[float, float], ...] = () # (latitude, longitude)
|
|
navigation_active: bool = False
|
|
satellites: int | None = None
|
|
gps_wait_seconds: float = 0.0
|
|
destination: str = '' # where navigation is going: "Home", "Work" or the place name
|
|
source: str = '' # where the fix comes from: comma, car or phone
|
|
|
|
|
|
class TileReader:
|
|
"""Road tiles from disk, decoded on a worker thread; the render thread only reads the cache."""
|
|
|
|
def __init__(self, root: Path | None = None, clock: Callable[[], float] = time.monotonic):
|
|
from openpilot.starpilot.navigation.offline_roads import TileStore
|
|
self.store = TileStore(root)
|
|
self.clock = clock
|
|
self._lock = threading.Condition()
|
|
self._tiles: OrderedDict[TileKey, RoadTile] = OrderedDict()
|
|
self._missing: dict[TileKey, float] = {}
|
|
self._wanted: tuple[TileKey, ...] = ()
|
|
self._jobs: list[Callable[[], None]] = []
|
|
self.generation = 0
|
|
self._stopped = False
|
|
self._thread = threading.Thread(target=self._run, name="map-tiles", daemon=True)
|
|
self._thread.start()
|
|
|
|
def want(self, keys: Sequence[TileKey]) -> None:
|
|
with self._lock:
|
|
self._wanted = tuple(keys)
|
|
self._lock.notify()
|
|
|
|
def submit(self, job: Callable[[], None]) -> None:
|
|
with self._lock:
|
|
self._jobs = [job] # only the newest canvas matters
|
|
self._lock.notify()
|
|
|
|
def snapshot(self, keys: Sequence[TileKey]) -> dict[TileKey, RoadTile]:
|
|
with self._lock:
|
|
return {key: self._tiles[key] for key in keys if key in self._tiles}
|
|
|
|
def _next_load(self) -> TileKey | None:
|
|
now = self.clock()
|
|
for key in self._wanted:
|
|
if key not in self._tiles and self._missing.get(key, -math.inf) <= now:
|
|
return key
|
|
return None
|
|
|
|
def _run(self) -> None:
|
|
while True:
|
|
with self._lock:
|
|
while not self._stopped and not self._jobs and self._next_load() is None:
|
|
self._lock.wait(timeout=MISSING_RETRY_S / 2)
|
|
if self._stopped:
|
|
return
|
|
key = self._next_load()
|
|
job = self._jobs.pop() if key is None and self._jobs else None
|
|
if key is not None:
|
|
raw = self.store.read(key)
|
|
try:
|
|
tile = decode_road_tile(key, raw) if raw is not None else None
|
|
except TileFormatError:
|
|
tile = None
|
|
with self._lock:
|
|
if tile is None:
|
|
self._missing[key] = self.clock() + MISSING_RETRY_S
|
|
else:
|
|
self._missing.pop(key, None)
|
|
self._tiles[key] = tile
|
|
self._tiles.move_to_end(key)
|
|
while len(self._tiles) > max(TILE_CACHE, len(self._wanted)):
|
|
self._tiles.popitem(last=False)
|
|
self.generation += 1
|
|
elif job is not None:
|
|
try:
|
|
job()
|
|
except Exception:
|
|
pass
|
|
|
|
def close(self) -> None:
|
|
with self._lock:
|
|
self._stopped = True
|
|
self._lock.notify()
|
|
self._thread.join(timeout=1.0)
|
|
|
|
|
|
# ---------------------------------------------------------------- shader
|
|
|
|
MASK_VERTEX = GL_VERSION + """
|
|
in vec3 vertexPosition;
|
|
in vec2 vertexTexCoord;
|
|
in vec4 vertexColor;
|
|
out vec2 fragTexCoord;
|
|
uniform mat4 mvp;
|
|
void main() {
|
|
fragTexCoord = vertexTexCoord;
|
|
gl_Position = mvp * vec4(vertexPosition, 1.0);
|
|
}
|
|
"""
|
|
|
|
MASK_FRAGMENT = GL_VERSION + """
|
|
in vec2 fragTexCoord;
|
|
out vec4 finalColor;
|
|
uniform sampler2D texture0;
|
|
uniform sampler2D guidance;
|
|
uniform vec2 size; // widget pixels
|
|
uniform float radius; // corner radius in pixels
|
|
uniform float feather; // soft edge width in pixels
|
|
uniform float opacity;
|
|
void main() {
|
|
vec2 p = fragTexCoord * size;
|
|
vec2 q = abs(p - size * 0.5) - (size * 0.5 - vec2(radius));
|
|
float outside = length(max(q, 0.0)) + min(max(q.x, q.y), 0.0) - radius; // rounded-rect distance, < 0 inside
|
|
float edge = clamp(-outside / feather, 0.0, 1.0);
|
|
edge = edge * edge * (3.0 - 2.0 * edge);
|
|
// The glass fades a little toward the top so the road ahead blends into the camera.
|
|
float top = smoothstep(0.0, 0.22, fragTexCoord.y) * 0.35 + 0.65;
|
|
// A faint glass rim where the feather meets the solid part.
|
|
float rim = (1.0 - smoothstep(0.0, 1.6, abs(outside + feather))) * 0.14;
|
|
vec2 uv = vec2(fragTexCoord.x, 1.0 - fragTexCoord.y);
|
|
vec4 base = texture(texture0, uv) * (opacity * top);
|
|
vec4 route = texture(guidance, uv);
|
|
finalColor = (route + base * (1.0 - route.a)) * edge + vec4(rim * opacity * top) * (1.0 - route.a);
|
|
}
|
|
"""
|
|
|
|
|
|
# ---------------------------------------------------------------- overlay
|
|
|
|
class MapOverlay:
|
|
"""Owns two canvases (front/back), a widget texture and the mask shader."""
|
|
|
|
def __init__(self, root: Path | None = None, *, reader: TileReader | SampleTileReader | None = None, fonts=None,
|
|
clock: Callable[[], float] = time.monotonic):
|
|
self.reader = reader or TileReader(root, clock)
|
|
self.fonts = fonts
|
|
self.clock = clock
|
|
self._lock = threading.Lock()
|
|
self._ready: CanvasGeometry | None = None
|
|
self._pending: CanvasRequest | None = None
|
|
self._canvas: rl.RenderTexture | None = None
|
|
self._canvas_request: CanvasRequest | None = None
|
|
self._widget: rl.RenderTexture | None = None
|
|
self._guidance: rl.RenderTexture | None = None
|
|
self._route_layers: list[tuple[rl.Color, np.ndarray]] = []
|
|
self._guidance_key = None
|
|
self._guidance_at: tuple[float, float] | None = None
|
|
self._widget_size: tuple[int, int] | None = None
|
|
self._shader: rl.Shader | None = None
|
|
self._uniforms: dict[str, int] = {}
|
|
self._uniform_values = (rl.ffi.new("Vector2 *"), rl.ffi.new("float *"), rl.ffi.new("float *"), rl.ffi.new("float *"))
|
|
self._pending_at = -math.inf
|
|
self.has_roads = False
|
|
self._route_key: tuple | None = None
|
|
self._route_world: np.ndarray | None = None
|
|
self._route_last_shown: float | None = None # clock time the route was last available this navigation session
|
|
self._starting_since: float | None = None # clock time "Starting navigation" went up
|
|
self._generation_built = -1
|
|
# Smoothed display state.
|
|
self._world: tuple[float, float] | None = None
|
|
self._latitude = 0.0
|
|
self._bearing = 0.0
|
|
self._has_heading = False
|
|
self._m_per_px = ZOOM_SLOW_M_PER_PX
|
|
self._last_prepare: float | None = None
|
|
self.status = "waiting"
|
|
self._input = MapInput(None)
|
|
|
|
# ---- state
|
|
|
|
def _advance(self, data: MapInput, now: float) -> None:
|
|
self._input = data
|
|
self.status = "live" if data.fix is not None and 0 <= now - data.fix.monotonic <= fix_hold_seconds(data.source) else "waiting"
|
|
fix = data.fix
|
|
dt = 0.0 if self._last_prepare is None else min(0.2, max(0.0, now - self._last_prepare))
|
|
self._last_prepare = now
|
|
if fix is None:
|
|
return
|
|
target_world = world_xy(fix.latitude, fix.longitude)
|
|
age = min(PHONE_DEAD_RECKON_S if data.source == "phone" else DEAD_RECKON_S, max(0.0, now - fix.monotonic))
|
|
if fix.bearing is not None and fix.speed > MIN_HEADING_SPEED:
|
|
units = fix.speed * age / meters_per_tile(fix.latitude)
|
|
heading = math.radians(fix.bearing)
|
|
target_world = (target_world[0] + math.sin(heading) * units, target_world[1] - math.cos(heading) * units)
|
|
delta = (fix.bearing - self._bearing + 540.0) % 360.0 - 180.0
|
|
self._bearing = (self._bearing + delta * (min(1.0, dt * 6.0) if self._has_heading else 1.0)) % 360.0
|
|
self._has_heading = True
|
|
if self._world is None or math.hypot(target_world[0] - self._world[0], target_world[1] - self._world[1]) > 0.5:
|
|
self._world = target_world # first fix or a jump: no slow glide across town
|
|
else:
|
|
k = min(1.0, dt * 12.0)
|
|
self._world = (self._world[0] + (target_world[0] - self._world[0]) * k,
|
|
self._world[1] + (target_world[1] - self._world[1]) * k)
|
|
self._latitude = fix.latitude
|
|
target_zoom = zoom_for_speed(fix.speed) if math.isfinite(fix.speed) else self._m_per_px
|
|
self._m_per_px += (target_zoom - self._m_per_px) * min(1.0, dt * 0.8)
|
|
|
|
def _route(self, route: tuple[tuple[float, float], ...]) -> None:
|
|
if route is self._route_key:
|
|
return # MapFeed hands back the same tuple until the route changes
|
|
if route != self._route_key:
|
|
self._guidance_key = None
|
|
self._route_world = np.array([world_xy(lat, lon) for lat, lon in route], np.float64) if len(route) >= 2 else None
|
|
self._route_key = route
|
|
|
|
def _track_route(self, data: MapInput, now: float) -> None:
|
|
if self._route_world is not None:
|
|
self._route_last_shown = now
|
|
elif not data.navigation_active:
|
|
self._route_last_shown = None
|
|
if not data.navigation_active:
|
|
self._starting_since = None
|
|
elif self._waiting_for_route(data, now):
|
|
if self._starting_since is None:
|
|
self._starting_since = now
|
|
elif self._starting_since is not None and now - self._starting_since >= STARTING_OVERLAY_MIN_SECONDS:
|
|
self._starting_since = None
|
|
|
|
def _waiting_for_route(self, data: MapInput, now: float) -> bool:
|
|
"""Navigation is active without a route: at once on a fresh start, after a grace period once a route was lost."""
|
|
return (data.navigation_active and self._route_world is None and
|
|
(self._route_last_shown is None or now - self._route_last_shown >= ROUTE_STALE_OVERLAY_SECONDS))
|
|
|
|
def _starting_navigation(self) -> bool:
|
|
"""While waiting for the route, and for a short minimum so the card can be read when the route is quick."""
|
|
now = self.clock()
|
|
return self._waiting_for_route(self._input, now) or (
|
|
self._input.navigation_active and self._starting_since is not None and now - self._starting_since < STARTING_OVERLAY_MIN_SECONDS)
|
|
|
|
# ---- canvas planning
|
|
|
|
@staticmethod
|
|
def _reach(width: int, height: int) -> float:
|
|
"""Screen pixels from the car to the widget's farthest corner, at any rotation."""
|
|
return math.hypot(width / 2, height * max(CAR_ANCHOR, 1 - CAR_ANCHOR))
|
|
|
|
def _canvas_side(self, width: int, height: int) -> int:
|
|
return int(min(MAX_CANVAS, math.ceil(self._reach(width, height) * 2 * 1.6 * SUPERSAMPLE / 64) * 64))
|
|
|
|
def _needs_canvas(self, side: int) -> bool:
|
|
request = self._canvas_request
|
|
if request is None or request.side != side:
|
|
return True
|
|
assert self._world is not None
|
|
assert self._widget_size is not None
|
|
px_per_unit = meters_per_tile(self._latitude) * SUPERSAMPLE / request.m_per_px
|
|
drift = math.hypot(self._world[0] - request.center[0], self._world[1] - request.center[1]) * px_per_unit
|
|
reach = self._reach(*self._widget_size) * SUPERSAMPLE * self._m_per_px / request.m_per_px
|
|
ratio = request.m_per_px / self._m_per_px
|
|
return drift > (side / 2 - reach) * 0.5 or not 0.82 < ratio < 1.22 or self.reader.generation != self._generation_built
|
|
|
|
def _request(self, side: int) -> CanvasRequest:
|
|
assert self._world is not None
|
|
reach_m = side / SUPERSAMPLE * self._m_per_px * 0.75
|
|
reach = reach_m / meters_per_tile(self._latitude)
|
|
x, y = self._world
|
|
tiles = tuple(TileKey(DATA_ZOOM, tx % (1 << DATA_ZOOM), ty)
|
|
for ty in range(int(math.floor(y - reach)), int(math.floor(y + reach)) + 1)
|
|
for tx in range(int(math.floor(x - reach)), int(math.floor(x + reach)) + 1)
|
|
if 0 <= ty < 1 << DATA_ZOOM)
|
|
done, ahead = split_route(self._route_world, self._world) if self._route_world is not None else (None, None)
|
|
return CanvasRequest(self._world, self._m_per_px, side, ahead, done, tiles)
|
|
|
|
def _schedule(self, request: CanvasRequest) -> None:
|
|
self._pending = request
|
|
self._pending_at = self.clock()
|
|
self.reader.want(sorted(request.tiles, key=lambda k: (k.x + 0.5 - request.center[0]) ** 2 + (k.y + 0.5 - request.center[1]) ** 2))
|
|
generation = self.reader.generation
|
|
latitude = self._latitude
|
|
|
|
def job():
|
|
geometry = build_canvas(request, self.reader.snapshot(request.tiles), latitude)
|
|
with self._lock:
|
|
if self._pending is request:
|
|
self._ready = geometry
|
|
self._generation_built = generation
|
|
self.reader.submit(job)
|
|
|
|
# ---- GPU
|
|
|
|
def _ensure_targets(self, width: int, height: int, side: int) -> bool:
|
|
if self._widget_size != (width, height):
|
|
if self._widget is not None:
|
|
rl.unload_render_texture(self._widget)
|
|
if self._guidance is not None:
|
|
rl.unload_render_texture(self._guidance)
|
|
self._widget = rl.load_render_texture(width, height)
|
|
self._guidance = rl.load_render_texture(width, height)
|
|
self._widget_size = None
|
|
if not self._widget.id or not self._guidance.id:
|
|
for target in (self._widget, self._guidance):
|
|
if target.id:
|
|
rl.unload_render_texture(target)
|
|
self._widget = self._guidance = None
|
|
return False
|
|
self._widget_size = (width, height)
|
|
for target in (self._widget, self._guidance):
|
|
rl.set_texture_filter(target.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR)
|
|
if self._shader is None:
|
|
self._shader = rl.load_shader_from_memory(MASK_VERTEX, MASK_FRAGMENT)
|
|
if not self._shader.id:
|
|
raise RuntimeError("Map overlay shader unavailable")
|
|
self._uniforms = {name: rl.get_shader_location(self._shader, name) for name in ("size", "radius", "feather", "opacity", "guidance")}
|
|
return True
|
|
|
|
def _draw_canvas(self, geometry: CanvasGeometry) -> None:
|
|
side = geometry.request.side
|
|
if self._canvas is None or self._canvas.texture.width != side:
|
|
if self._canvas is not None:
|
|
rl.unload_render_texture(self._canvas)
|
|
self._canvas = rl.load_render_texture(side, side)
|
|
if not self._canvas.id:
|
|
self._canvas = None
|
|
return
|
|
rl.set_texture_filter(self._canvas.texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR)
|
|
rl.begin_texture_mode(self._canvas)
|
|
try:
|
|
rl.clear_background(rl.BLANK)
|
|
rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY)
|
|
for color, strip in geometry.layers:
|
|
strip = np.ascontiguousarray(strip, np.float32)
|
|
rl.draw_triangle_strip(rl.ffi.from_buffer("Vector2 *", strip), len(strip), color)
|
|
rl.end_blend_mode()
|
|
finally:
|
|
rl.end_texture_mode()
|
|
self._guidance_key = None
|
|
self._canvas_request = geometry.request
|
|
self.has_roads = geometry.tiles_missing < len(geometry.request.tiles)
|
|
|
|
def _puck(self, x: float, y: float) -> None:
|
|
rl.draw_circle_v(rl.Vector2(x, y), 26, premultiplied((150, 110, 255, 70)))
|
|
points = [(0, -19), (-14, 15), (0, 7), (14, 15)]
|
|
outline = [rl.Vector2(x + px * 1.25, y + py * 1.25) for px, py in points]
|
|
body = [rl.Vector2(x + px, y + py) for px, py in points]
|
|
for shape, color in ((outline, premultiplied((96, 70, 210))), (body, premultiplied((255, 255, 255)))):
|
|
rl.draw_triangle(shape[0], shape[1], shape[2], color)
|
|
rl.draw_triangle(shape[0], shape[2], shape[3], color)
|
|
|
|
def _compass(self, width: int, height: int) -> None:
|
|
"""A small needle in the top right that always points north."""
|
|
inset = max(22.0, min(width, height) * 0.09)
|
|
cx, cy = width - inset, inset
|
|
rl.draw_circle_v(rl.Vector2(cx, cy), 13, premultiplied((8, 12, 20, 170)))
|
|
angle = math.radians(-self._bearing)
|
|
def point(r, a):
|
|
return rl.Vector2(cx + r * math.sin(angle + a), cy - r * math.cos(angle + a))
|
|
rl.draw_triangle(point(10, 0), point(5, -2.3), point(5, 2.3), premultiplied((255, 110, 96)))
|
|
rl.draw_triangle(point(10, math.pi), point(5, math.pi + 2.3), point(5, math.pi - 2.3), premultiplied((210, 214, 224, 200)))
|
|
|
|
def _compose_layer(self, width: int, height: int, guidance: bool) -> None:
|
|
rl.begin_texture_mode(self._guidance if guidance else self._widget)
|
|
try:
|
|
rl.clear_background(rl.BLANK)
|
|
rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY)
|
|
if not guidance:
|
|
rl.draw_rectangle(0, 0, width, height, premultiplied(GLASS))
|
|
request = self._canvas_request
|
|
if self._canvas is not None and request is not None and self._world is not None:
|
|
px_per_unit = meters_per_tile(self._latitude) / request.m_per_px
|
|
zoom = request.m_per_px / self._m_per_px / SUPERSAMPLE
|
|
side = request.side
|
|
# The car's spot on the canvas, in canvas pixels.
|
|
car_x = (self._world[0] - request.center[0]) * px_per_unit * SUPERSAMPLE + side / 2
|
|
car_y = (self._world[1] - request.center[1]) * px_per_unit * SUPERSAMPLE + side / 2
|
|
anchor = rl.Vector2(width / 2, height * CAR_ANCHOR)
|
|
if guidance:
|
|
# Road geometry is cached across many frames. Guidance is trimmed at the
|
|
# interpolated car, but only again once the car has moved a few screen
|
|
# pixels: the driven bit in between stays under the puck. Re-trimming
|
|
# every frame halved the car view's frame rate on long routes.
|
|
moved = math.inf if self._guidance_at is None else math.hypot(self._world[0] - self._guidance_at[0],
|
|
self._world[1] - self._guidance_at[1])
|
|
slack = GUIDANCE_SLACK_PX * self._m_per_px / meters_per_tile(self._latitude)
|
|
if self._guidance_key != id(request) or moved > slack:
|
|
_, ahead = split_route(self._route_world, self._world)
|
|
self._route_layers = route_guidance(ahead, request, self._latitude)
|
|
self._guidance_key = id(request)
|
|
self._guidance_at = self._world
|
|
rl.rl_push_matrix()
|
|
try:
|
|
rl.rl_translatef(anchor.x, anchor.y, 0)
|
|
rl.rl_rotatef(-self._bearing, 0, 0, 1)
|
|
rl.rl_scalef(zoom, zoom, 1)
|
|
rl.rl_translatef(-car_x, -car_y, 0)
|
|
for color, strip in self._route_layers:
|
|
strip = np.ascontiguousarray(strip, np.float32)
|
|
rl.draw_triangle_strip(rl.ffi.from_buffer("Vector2 *", strip), len(strip), color)
|
|
finally:
|
|
rl.rl_pop_matrix()
|
|
if self.status == "live":
|
|
self._puck(anchor.x, anchor.y)
|
|
self._compass(width, height)
|
|
else:
|
|
rl.draw_texture_pro(self._canvas.texture, rl.Rectangle(0, 0, side, -side),
|
|
rl.Rectangle(anchor.x, anchor.y, side * zoom, side * zoom),
|
|
rl.Vector2(car_x * zoom, car_y * zoom), -self._bearing, rl.WHITE)
|
|
rl.end_blend_mode()
|
|
finally:
|
|
rl.end_texture_mode()
|
|
|
|
def _compose(self, width: int, height: int) -> None:
|
|
self._compose_layer(width, height, False)
|
|
self._compose_layer(width, height, True)
|
|
|
|
# ---- public
|
|
|
|
def prepare(self, data: MapInput, width: int, height: int) -> None:
|
|
"""Outside any render target: advance, rebuild the canvas if needed and compose the widget."""
|
|
width, height = max(16, int(width)), max(16, int(height))
|
|
if not self._ensure_targets(width, height, 0):
|
|
return
|
|
now = self.clock()
|
|
self._advance(data, now)
|
|
self._route(data.route)
|
|
self._track_route(data, now)
|
|
if self._world is not None:
|
|
side = self._canvas_side(width, height)
|
|
with self._lock:
|
|
ready, self._ready = self._ready, None
|
|
if ready is not None:
|
|
self._draw_canvas(ready)
|
|
in_flight = self._pending is not None and self._canvas_request is not self._pending and now - self._pending_at < 2.0
|
|
if not in_flight and self._needs_canvas(side):
|
|
self._schedule(self._request(side))
|
|
self._compose(width, height)
|
|
|
|
def draw(self, rect: rl.Rectangle, opacity: float) -> None:
|
|
"""Inside the frame: the composed widget, rounded, feathered and faded."""
|
|
if self._widget is None or self._shader is None:
|
|
return
|
|
assert self._widget_size is not None
|
|
width, height = self._widget_size
|
|
size, radius, feather, alpha = self._uniform_values
|
|
size.x, size.y = width, height
|
|
radius[0] = min(width, height) * 0.09
|
|
feather[0] = max(10.0, min(width, height) * 0.07)
|
|
alpha[0] = max(0.0, min(1.0, opacity))
|
|
rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY)
|
|
rl.begin_shader_mode(self._shader)
|
|
try:
|
|
# Typed pointers: pyray drops float arrays and by-value structs passed as uniform data.
|
|
vec2, scalar = rl.ShaderUniformDataType.SHADER_UNIFORM_VEC2, rl.ShaderUniformDataType.SHADER_UNIFORM_FLOAT
|
|
for name, value, kind in (("size", size, vec2), ("radius", radius, scalar), ("feather", feather, scalar), ("opacity", alpha, scalar)):
|
|
rl.set_shader_value(self._shader, self._uniforms[name], value, kind)
|
|
rl.set_shader_value_texture(self._shader, self._uniforms["guidance"], self._guidance.texture)
|
|
rl.draw_texture_pro(self._widget.texture, rl.Rectangle(0, 0, width, height), rect, rl.Vector2(0, 0), 0, rl.WHITE)
|
|
finally:
|
|
rl.end_shader_mode()
|
|
rl.end_blend_mode()
|
|
if self.status == "waiting":
|
|
self._draw_gps_status(rect)
|
|
return
|
|
self._draw_source_chip(rect)
|
|
if self._starting_navigation():
|
|
self._draw_starting_navigation(rect)
|
|
return
|
|
text = "" if self.has_roads or self._canvas_request is None else "No map data here yet"
|
|
if text and self.fonts is not None:
|
|
from openpilot.starpilot.ui.presentation import FontRole
|
|
measured = self.fonts.measure(text, FontRole.MEDIUM, 26)
|
|
self.fonts.draw(text, FontRole.MEDIUM, 26, rect.x + (rect.width - measured.width) / 2,
|
|
rect.y + rect.height * CAR_ANCHOR + 40, rl.Color(225, 228, 238, round(200 * opacity)))
|
|
|
|
def _draw_source_chip(self, rect: rl.Rectangle) -> None:
|
|
"""Where the fix comes from, left of the compass; solid whatever the map's opacity so it always reads."""
|
|
label = SOURCE_LABELS.get(self._input.source)
|
|
if label is None or self.fonts is None:
|
|
return
|
|
from openpilot.starpilot.ui.presentation import FontRole
|
|
scale = max(.7, min(1.0, rect.width / 560, rect.height / 420))
|
|
size, pad_x, height = 17 * scale, 10 * scale, 26 * scale
|
|
inset = max(22.0, min(rect.width, rect.height) * 0.09)
|
|
measured = self.fonts.measure(label, FontRole.SEMI_BOLD, size)
|
|
width = measured.width + 2 * pad_x
|
|
chip = rl.Rectangle(rect.x + rect.width - inset - 13 - 8 * scale - width, rect.y + inset - height / 2, width, height)
|
|
if chip.x < rect.x + 4:
|
|
return
|
|
rl.draw_rectangle_rounded(chip, 1.0, 10, rl.Color(15, 13, 23, 255))
|
|
rl.draw_rectangle_rounded_lines_ex(chip, 1.0, 10, 1.5, rl.Color(150, 140, 200, 255))
|
|
self.fonts.draw(label, FontRole.SEMI_BOLD, size, chip.x + pad_x, chip.y + (height - measured.height) / 2, rl.Color(240, 242, 250, 255))
|
|
|
|
def _draw_starting_navigation(self, rect: rl.Rectangle) -> None:
|
|
"""Shown while navigation is starting; solid whatever the map's opacity so it always reads."""
|
|
if self.fonts is None:
|
|
return
|
|
from openpilot.starpilot.ui.presentation import FontRole
|
|
scale = max(.7, min(1.0, rect.width / 560, rect.height / 420))
|
|
inset, padding = 20 * scale, 14 * scale
|
|
width = rect.width - 2 * inset
|
|
destination = self._input.destination.strip()
|
|
if len(destination) > DESTINATION_MAX_CHARS:
|
|
destination = destination[:DESTINATION_MAX_CHARS - 1].rstrip() + '…'
|
|
rows = [('Starting navigation', FontRole.SEMI_BOLD, 26)]
|
|
if destination:
|
|
rows.append((f'to {destination}', FontRole.MEDIUM, 22))
|
|
if self._route_world is None:
|
|
rows.append(('Finding your route…', FontRole.NORMAL, 18))
|
|
scale = min(scale, *(max(1, width - 2 * padding) / self.fonts.measure(text, role, size).width
|
|
for text, role, size in rows))
|
|
height = (len(rows) * 32 + 22) * scale
|
|
card = rl.Rectangle(rect.x + inset, rect.y + rect.height - inset - height, width, height)
|
|
rl.draw_rectangle_rounded(card, .2, 8, rl.Color(15, 13, 23, 255))
|
|
y = card.y + 10 * scale
|
|
for text, role, size in rows:
|
|
measured = self.fonts.measure(text, role, size * scale)
|
|
self.fonts.draw(text, role, size * scale, card.x + (card.width - measured.width) / 2, y,
|
|
rl.Color(225, 228, 238, 255))
|
|
y += 32 * scale
|
|
|
|
def _draw_gps_status(self, rect: rl.Rectangle) -> None:
|
|
"""AAComma's acquisition overlay, fitted to the bottom of the projected map."""
|
|
if self.fonts is None:
|
|
return
|
|
from openpilot.starpilot.ui.presentation import FontRole
|
|
data = self._input
|
|
elapsed = max(0, int(data.gps_wait_seconds))
|
|
satellites = ('Searching for satellites' if data.satellites is None else
|
|
f"{data.satellites} satellite{'s' if data.satellites != 1 else ''} locked")
|
|
rows = [('Navigation active', FontRole.SEMI_BOLD, 22)] if data.navigation_active else []
|
|
rows += [('Waiting for GPS', FontRole.MEDIUM, 26), (f'{satellites} • {elapsed // 60}:{elapsed % 60:02d}', FontRole.NORMAL, 18)]
|
|
scale = max(.7, min(1.0, rect.width / 560, rect.height / 420))
|
|
inset, padding = 20 * scale, 14 * scale
|
|
width = rect.width - 2 * inset
|
|
# Keep every line inside even the smallest allowed map widget.
|
|
scale = min(scale, *(max(1, width - 2 * padding) / self.fonts.measure(text, role, size).width
|
|
for text, role, size in rows))
|
|
height = (len(rows) * 30 + 30) * scale
|
|
card = rl.Rectangle(rect.x + inset, rect.y + rect.height - inset - height, width, height)
|
|
rl.draw_rectangle_rounded(card, .2, 8, rl.Color(15, 13, 23, 235))
|
|
y = card.y + 10 * scale
|
|
for text, role, size in rows:
|
|
measured = self.fonts.measure(text, role, size * scale)
|
|
self.fonts.draw(text, role, size * scale, card.x + (card.width - measured.width) / 2, y,
|
|
rl.Color(225, 228, 238, 255))
|
|
y += 30 * scale
|
|
track = rl.Rectangle(card.x + padding, y + 2 * scale, card.width - 2 * padding, 6 * scale)
|
|
rl.draw_rectangle_rounded(track, 1, 8, rl.Color(66, 58, 82, 255))
|
|
fill = rl.Rectangle(track.x, track.y, max(track.height, track.width * acquisition_progress(data.satellites)), track.height)
|
|
rl.draw_rectangle_rounded(fill, 1, 8, rl.Color(199, 174, 247, 255))
|
|
|
|
def close(self) -> None:
|
|
self.reader.close()
|
|
if rl.is_window_ready():
|
|
for target in (self._canvas, self._widget, self._guidance):
|
|
if target is not None:
|
|
rl.unload_render_texture(target)
|
|
if self._shader is not None and self._shader.id:
|
|
rl.unload_shader(self._shader)
|
|
self._canvas = self._widget = self._guidance = None
|
|
self._route_layers = []
|
|
self._shader = None
|
|
|
|
|
|
# ---------------------------------------------------------------- live input
|
|
|
|
def fix_hold_seconds(source: str) -> float:
|
|
return (PHONE_MAX_AGE_NS if source == "phone" else GPS_MAX_AGE_NS) / 1e9
|
|
|
|
|
|
class MapFeed:
|
|
"""The same GPS choice as navigation (either receiver, then the car's own), and the active route."""
|
|
|
|
def __init__(self, sm=None, *, acquisition=None):
|
|
from openpilot.starpilot.gps.source import GPS_SERVICES
|
|
if sm is None:
|
|
from openpilot.cereal import messaging
|
|
sm = messaging.SubMaster(list(GPS_SERVICES))
|
|
self.sm = sm
|
|
self._fix: MapFix | None = None
|
|
self._source = ''
|
|
self._route_stamp = None
|
|
self._route_valid_until = 0
|
|
self._route: tuple[tuple[float, float], ...] = ()
|
|
self._navigation_active = False
|
|
self._destination = ''
|
|
self._acquisition = acquisition if acquisition is not None else GpsAcquisition()
|
|
self._acquisition_polled = -math.inf
|
|
|
|
def read(self, navigation_sm=None, *, navigation_requested=None, destination=None) -> MapInput:
|
|
from openpilot.starpilot.gps.source import bearing, select_source
|
|
self.sm.update(0)
|
|
now = time.monotonic()
|
|
now_ns = time.monotonic_ns()
|
|
chosen = select_source(self.sm, now_ns)
|
|
selected = chosen[1] if chosen is not None else None
|
|
if selected is not None:
|
|
stamp, gps = selected
|
|
speed = float(getattr(gps, "speed", float("nan")))
|
|
if -85 < gps.latitude < 85:
|
|
# The fix's own timestamp, so dead reckoning starts from when it was measured.
|
|
self._fix = MapFix(gps.latitude, gps.longitude, bearing(gps), max(0.0, speed) if math.isfinite(speed) else math.nan, stamp / 1e9)
|
|
self._source = chosen[0]
|
|
if self._fix is not None and now - self._fix.monotonic > max(5.0, fix_hold_seconds(self._source)):
|
|
self._fix = None
|
|
fresh = self._fix is not None and 0 <= now - self._fix.monotonic <= fix_hold_seconds(self._source)
|
|
if fresh:
|
|
self._acquisition.reset()
|
|
self._acquisition_polled = -math.inf
|
|
elif now - self._acquisition_polled >= .5:
|
|
self._acquisition_polled = now
|
|
self._acquisition.update(now)
|
|
route: tuple[tuple[float, float], ...] = ()
|
|
active = False
|
|
if navigation_sm is not None:
|
|
# Decode only new navigation messages and retain the same route tuple
|
|
# while its points are unchanged. GPS acquisition still advances each read.
|
|
try:
|
|
stamp = (navigation_sm.logMonoTime["starpilotNavigation"], navigation_sm.valid["starpilotNavigation"])
|
|
except Exception:
|
|
stamp = None
|
|
if stamp is None or stamp != self._route_stamp:
|
|
self._route_stamp = stamp
|
|
try:
|
|
from openpilot.starpilot.navigation.wire import navigation_state
|
|
nav = navigation_state(navigation_sm["starpilotNavigation"]) if navigation_sm.valid["starpilotNavigation"] else None
|
|
active = bool(nav is not None and nav.enabled and nav.destinationName)
|
|
self._destination = nav.destinationName if active else ''
|
|
self._route_valid_until = 0
|
|
if (nav is not None and nav.enabled and nav.status in ("guiding", "arrived") and
|
|
0 < nav.frameMonoTime <= now_ns <= nav.frameMonoTime + 3_000_000_000):
|
|
self._route_valid_until = nav.frameMonoTime + 3_000_000_000
|
|
route = tuple((point.latitude, point.longitude) for point in nav.route)
|
|
except Exception:
|
|
route = ()
|
|
if route != self._route:
|
|
self._route = route
|
|
self._navigation_active = active
|
|
if now_ns > self._route_valid_until:
|
|
self._route = ()
|
|
route, active = self._route, self._navigation_active
|
|
acquisition_elapsed = 0.0
|
|
if not fresh:
|
|
acquisition_since = self._acquisition.since
|
|
assert acquisition_since is not None # update() starts the acquisition clock.
|
|
acquisition_elapsed = now - acquisition_since
|
|
return MapInput(self._fix, route, active if navigation_requested is None else navigation_requested,
|
|
None if fresh else self._acquisition.satellites(now),
|
|
acquisition_elapsed, self._destination if destination is None else destination,
|
|
self._source if self._fix is not None else '')
|
|
|
|
|
|
# ---------------------------------------------------------------- parked preview
|
|
|
|
SAMPLE_TILE = TileKey(DATA_ZOOM, 2950, 6427)
|
|
SAMPLE_CAR = (1450.0, 2930.0) # tile units inside SAMPLE_TILE, south of a left turn
|
|
SAMPLE_BEARING = 0.0
|
|
|
|
|
|
def _sample_roads() -> dict[int, list[np.ndarray]]:
|
|
"""A believable neighborhood for the parked layout preview; drawn from no real map."""
|
|
t = np.linspace(-4500, 8600, 90)
|
|
roads: dict[int, list[np.ndarray]] = {
|
|
MOTORWAY: [np.stack((2700 + 520 * np.sin(t / 1900), t), axis=1)],
|
|
MAJOR: [np.array([[-4500, 2300], [1200, 2620], [8600, 2050]]), np.array([[1450, 8600], [1450, 2620], [1700, -4500]])],
|
|
STREET: [], MINOR: [], LINK: [np.array([[2400, 1500], [2650, 1900], [3050, 2200]])],
|
|
}
|
|
angle = math.radians(8)
|
|
rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
|
|
for i in range(-12, 22):
|
|
offset = i * 420.0
|
|
roads[STREET].append(np.array([[offset, -4500], [offset + 60, 8600]]) @ rotation.T)
|
|
roads[STREET].append(np.array([[-4500, offset], [8600, offset - 90]]) @ rotation.T)
|
|
roads[MINOR].append(np.array([[offset + 210, offset - 300], [offset + 330, offset - 120], [offset + 210, offset + 60]]))
|
|
return roads
|
|
|
|
|
|
class SampleTileReader:
|
|
"""TileReader's interface over one synthetic tile; jobs run inline."""
|
|
|
|
def __init__(self):
|
|
raw = encode_road_tile(_sample_roads())
|
|
self._tile = decode_road_tile(SAMPLE_TILE, raw)
|
|
self.generation = 1
|
|
|
|
def want(self, keys) -> None:
|
|
pass
|
|
|
|
def submit(self, job) -> None:
|
|
job()
|
|
|
|
def snapshot(self, keys) -> dict[TileKey, RoadTile]:
|
|
return {key: self._tile for key in keys if key == SAMPLE_TILE}
|
|
|
|
def close(self) -> None:
|
|
pass
|
|
|
|
|
|
def sample_input() -> MapInput:
|
|
from openpilot.starpilot.navigation.road_tiles import lat_lon
|
|
def world(x, y):
|
|
return lat_lon(SAMPLE_TILE.x + x / EXTENT, SAMPLE_TILE.y + y / EXTENT)
|
|
latitude, longitude = world(*SAMPLE_CAR)
|
|
route = tuple(world(x, y) for x, y in ((1450, 4200), (1450, 2620), (1200, 2620), (-1800, 2460)))
|
|
return MapInput(MapFix(latitude, longitude, SAMPLE_BEARING, 8.0, time.monotonic()), route)
|