From 2ac005ab1670ea4f3caaf2a38b35d20f3ad82cc5 Mon Sep 17 00:00:00 2001 From: malice Date: Wed, 27 May 2026 09:53:26 -0600 Subject: [PATCH] navi-offroute: unified cost layers + transition cells (Phase 3) (#39) Co-authored-by: mj Co-authored-by: Claude Opus 4.7 (1M context) --- backend/services/navi_offroute/cost.py | 69 ++++++ .../navi_offroute/tests/test_offroute.py | 125 +++++++++++ backend/services/navi_offroute/transitions.py | 206 ++++++++++++++++++ 3 files changed, 400 insertions(+) create mode 100644 backend/services/navi_offroute/transitions.py diff --git a/backend/services/navi_offroute/cost.py b/backend/services/navi_offroute/cost.py index e5049db..a3f2ab5 100755 --- a/backend/services/navi_offroute/cost.py +++ b/backend/services/navi_offroute/cost.py @@ -198,6 +198,12 @@ MODE_PROFILES: Dict[str, ModeProfile] = { # Pragmatic mode friction multiplier for private land PRAGMATIC_BARRIER_MULTIPLIER = 5.0 +# Mode-switch transition penalties (seconds), unified-graph Auto (spec §4; used by transitions.py). +TRANSITION_COST_PARKING_S = 60.0 # park & switch at a lot +TRANSITION_COST_TRAILHEAD_S = 30.0 # stage at a trailhead +TRANSITION_COST_ROAD_TERMINUS_S = 60.0 # leave/meet vehicle at road end +TRANSITION_COST_SURFACE_CHANGE_S = 0.0 # surface boundary, free swap + # ═══════════════════════════════════════════════════════════════════════════════ # COST GRID COMPUTATION @@ -505,6 +511,69 @@ def compute_cost_grid( return cost +# ═══════════════════════════════════════════════════════════════════════════════ +# UNIFIED COST LAYERS (unified-graph Auto, spec §3.2) +# ═══════════════════════════════════════════════════════════════════════════════ + +def compute_unified_cost_layers( + elevation: np.ndarray, + friction: Optional[np.ndarray], + friction_raw: Optional[np.ndarray], + trails: Optional[np.ndarray], + wilderness: Optional[np.ndarray], + meta: dict, + modes=("foot", "2w", "4w", "vehicle"), + boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic", + endpoint_line=None, + valhalla_url=None, +) -> dict: + """Per-mode inflated cost layers + mode-transition cells for one Auto search + (spec §3.2 / §4 / §5). Returns {"cost_mult": {mode: ndarray}, "transition_cells": + [(row, col, from_idx, to_idx, cost_s), ...], "meta": {...DEMReader meta, + + "boundary_mode"}}. + + Rasters are INJECTED, not fetched: the raster IO lives on the router's reader + objects (router.py::_pathfind_wilderness) and is not duplicated — Phase 4 passes + elevation/friction/trails/wilderness + DEMReader `meta` straight in; tests pass + synthetic arrays. Each mode's multiplier comes from compute_cost_multiplier_grid(...) + then inflate_cost_multiplier(...). boundary_mode governs barrier/MVUM rules, which + are PER-EDGE in the kernel (§9), so it is threaded into the returned meta for Phase 4 + rather than into compute_cost_multiplier_grid (which has no such param, unchanged). + """ + from .astar import inflate_cost_multiplier + from .transitions import gather_transition_cells + + cell_size_m = float(meta["cell_size_m"]) + cost_mult = {} + for mode in modes: + m = compute_cost_multiplier_grid( + elevation, + cell_size_lat_m=cell_size_m, + cell_size_lon_m=cell_size_m, + friction=friction, + friction_raw=friction_raw, + wilderness=wilderness, + mode=mode, + ) + cost_mult[mode] = inflate_cost_multiplier(m) + + transition_cells = gather_transition_cells( + meta, + endpoint_line=endpoint_line, + trail_grid=trails, + elevation=elevation, + valhalla_url=valhalla_url, + ) + + out_meta = dict(meta) + out_meta["boundary_mode"] = boundary_mode + return { + "cost_mult": cost_mult, + "transition_cells": transition_cells, + "meta": out_meta, + } + + # ═══════════════════════════════════════════════════════════════════════════════ # LEGACY API (backward compatibility) # ═══════════════════════════════════════════════════════════════════════════════ diff --git a/backend/services/navi_offroute/tests/test_offroute.py b/backend/services/navi_offroute/tests/test_offroute.py index 1fb8881..074385f 100644 --- a/backend/services/navi_offroute/tests/test_offroute.py +++ b/backend/services/navi_offroute/tests/test_offroute.py @@ -1177,3 +1177,128 @@ def test_multimode_heuristic_admissibility(): assert h <= true_cost + 1e-6 sampled += 1 assert sampled > 0 # the sweep actually exercised reachable states + + +# ── PHASE 3 — unified cost layers + transition cells (cost.py + transitions.py) ── +import os as _os +import time as _time +import math as _math +import numpy as _p3np +from services.navi_offroute.cost import ( + compute_unified_cost_layers as _cu_layers, + compute_cost_multiplier_grid as _ccmg, +) +from services.navi_offroute.astar import inflate_cost_multiplier as _inflate +import services.navi_offroute.transitions as _trans + + +def _p3_meta(rows, cols, cell_m=30.0): + """Synthetic DEMReader-shape meta near lat 40 (mirrors shared/dem.py meta keys).""" + dlat = cell_m / 111000.0 + dlon = cell_m / (111000.0 * _math.cos(_math.radians(40.0))) + return { + "bounds": (40.0, 40.0 + rows * dlat, -111.0, -111.0 + cols * dlon), + "pixel_size_lat": -dlat, + "pixel_size_lon": dlon, + "origin_lat": 40.0 + rows * dlat, # top edge (row 0) + "origin_lon": -111.0, + "cell_size_m": cell_m, + "shape": (rows, cols), + } + + +def test_unified_cost_layers_per_mode_parity(): + """cost_mult[mode] == inflate(compute_cost_multiplier_grid(mode)) for each mode.""" + rows, cols = 24, 30 + rng = _p3np.random.default_rng(7) + elevation = (1000.0 + rng.normal(0, 30, (rows, cols))).astype(_p3np.float64) + elevation[3, 4] = _p3np.nan # exercise inf handling + friction = (1.0 + rng.random((rows, cols))).astype(_p3np.float64) + friction_raw = rng.choice([10, 20, 30, 60], size=(rows, cols)).astype(_p3np.uint8) + trails = _p3np.zeros((rows, cols), _p3np.uint8); trails[10, :] = 5 + wilderness = _p3np.zeros((rows, cols), _p3np.uint8); wilderness[0:3, 0:3] = 255 + meta = _p3_meta(rows, cols) + cm = float(meta["cell_size_m"]) + layers = _cu_layers( + elevation, friction, friction_raw, trails, wilderness, meta, + modes=("foot", "2w", "4w", "vehicle"), boundary_mode="pragmatic", + endpoint_line=None) + assert set(layers["cost_mult"]) == {"foot", "2w", "4w", "vehicle"} + assert layers["meta"]["boundary_mode"] == "pragmatic" + for mode in ("foot", "2w", "4w", "vehicle"): + expected = _inflate(_ccmg( + elevation, cell_size_lat_m=cm, cell_size_lon_m=cm, + friction=friction, friction_raw=friction_raw, + wilderness=wilderness, mode=mode)) + got = layers["cost_mult"][mode] + assert _p3np.array_equal(_p3np.isinf(got), _p3np.isinf(expected)) + fin = ~_p3np.isinf(expected) + assert _p3np.allclose(got[fin], expected[fin]) + + +def test_road_terminus_transitions_pure_raster(): + """A road row ending mid-grid yields foot↔vehicle termini at 60 s, no DB.""" + rows, cols = 10, 10 + trail_grid = _p3np.zeros((rows, cols), _p3np.uint8) + trail_grid[5, 0:6] = 5 # road cols 0..5; col 6 is off-network + meta = _p3_meta(rows, cols) + tuples = _trans.road_terminus_transitions(meta, trail_grid) + cells = {} + for (lat, lon, fm, tm, cost_s) in tuples: + cells.setdefault(_trans._latlon_to_pixel(lat, lon, meta), []).append((fm, tm, cost_s)) + assert set(cells) == {(5, c) for c in range(6)} # all row-5 road cells border off-network + f, v = _trans.MODE_INDEX["foot"], _trans.MODE_INDEX["vehicle"] + for edges in cells.values(): + assert sorted(edges) == sorted([(f, v, 60.0), (v, f, 60.0)]) + + +def test_transition_cap_closest_15(monkeypatch): + """>15 parking lots within 5 km -> only the closest 15 (by perp distance) survive.""" + line = ((40.0, -111.0), (40.0, -110.0)) # ~east-west; lat offset = perp distance, all <5 km + records = [{"lat": 40.0 + 0.0005 * k, "lon": -110.5, "name": f"P{k}", "access": "yes"} + for k in range(1, 21)] + + class _StubParking: + def query_parking_near_line(self, coords, buffer_m=2000): + return records + monkeypatch.setattr(_trans, "load_parking_index", lambda *a, **k: _StubParking()) + raw = _trans.parking_transitions_near_line(line, buffer_m=5000) + capped = _trans._cap_candidates(raw, line) + surviving_lats = sorted({round(t[0], 6) for t in capped}) + expected_lats = sorted({round(40.0 + 0.0005 * k, 6) for k in range(1, 16)}) + assert surviving_lats == expected_lats # the closest 15 points + assert len(capped) == 15 * 6 # 6 directed tuples per lot + + +def test_compute_unified_cost_layers_perf(): + """≤1 s to build 4 cost layers + transition cells for a ~50 km bbox (spec §5 gate). + Requires the real parking/trailhead DBs + a reachable Valhalla; skips otherwise.""" + from services.navi_offroute.mvum_parking import parking_db_path + from services.navi_offroute.mvum import navi_db_path + + valhalla_url = _os.environ.get("NAVI_OFFROUTE_VALHALLA_URL", "http://localhost:8002") + if not (_os.path.exists(parking_db_path()) and _os.path.exists(navi_db_path())): + pytest.skip("parking/trailhead DBs not present locally — skipping perf gate") + try: + import requests + requests.get(f"{valhalla_url}/status", timeout=1).raise_for_status() + except Exception as e: + pytest.skip(f"Valhalla not reachable at {valhalla_url}: {e}") + + cell_m = 30.0 + n = int(50_000 / cell_m) # ~50 km / 30 m + elevation = _p3np.full((n, n), 1000.0, dtype=_p3np.float64) + friction = _p3np.ones((n, n), dtype=_p3np.float64) + friction_raw = _p3np.full((n, n), 30, dtype=_p3np.uint8) + trails = _p3np.zeros((n, n), _p3np.uint8); trails[n // 2, :] = 5 + wilderness = _p3np.zeros((n, n), _p3np.uint8) + meta = _p3_meta(n, n, cell_m) + south, north, west, east = meta["bounds"] + t0 = _time.perf_counter() + layers = _cu_layers( + elevation, friction, friction_raw, trails, wilderness, meta, + modes=("foot", "2w", "4w", "vehicle"), boundary_mode="pragmatic", + endpoint_line=((south, west), (north, east)), valhalla_url=valhalla_url) + elapsed = _time.perf_counter() - t0 + assert set(layers["cost_mult"]) == {"foot", "2w", "4w", "vehicle"} + assert elapsed <= 1.0, f"unified cost layers build took {elapsed:.3f}s > 1.0s" diff --git a/backend/services/navi_offroute/transitions.py b/backend/services/navi_offroute/transitions.py new file mode 100644 index 0000000..df65847 --- /dev/null +++ b/backend/services/navi_offroute/transitions.py @@ -0,0 +1,206 @@ +"""Transition-cell sourcing for unified-graph Auto (spec §4–§5). + +Gathers mode-switch cells from four sources (parking, trailheads, road termini, +surface-change boundaries), maps each to a DEM grid pixel, de-dupes, and applies the +per-type closest-15-within-5 km cap. Returns the flat directed (row, col, from_idx, +to_idx, cost_s) list astar_multigoal_multimode consumes. Pure sourcing + grid mapping; +no raster math (cost.py), no router wiring (Phase 4). lat/lon → pixel mirrors +DEMReader.latlon_to_pixel (shared/dem.py). +""" +import math + +import numpy as np + +from .cost import ( + TRANSITION_COST_PARKING_S, + TRANSITION_COST_TRAILHEAD_S, + TRANSITION_COST_ROAD_TERMINUS_S, + TRANSITION_COST_SURFACE_CHANGE_S, +) +from .mvum_parking import load_parking_index +from .mvum_transitions import load_trailheads +from .mvum_surface_change import get_surface_change_candidates + +# Fixed mode ordering (spec §2.1; matches astar_multigoal_multimode). +MODE_INDEX = {"foot": 0, "2w": 1, "4w": 2, "vehicle": 3} + +_CAP_PER_TYPE = 15 # §5: keep the closest 15 cells per transition type +_CAP_RADIUS_M = 5000.0 # §5: within 5 km of the endpoint line +_EARTH_R_M = 6_371_000.0 +_BLOCKED_ACCESS = frozenset({"private", "no", "permit"}) # defensive; index already drops these + + +# ── lat/lon ↔ pixel (mirror DEMReader, shared/dem.py) ─────────────────────────── + +def _latlon_to_pixel(lat, lon, meta): + # Mirrors DEMReader.latlon_to_pixel; +1e-6 stabilises the center round-trip (float error). + row = int((meta["origin_lat"] - lat) / abs(meta["pixel_size_lat"]) + 1e-6) + col = int((lon - meta["origin_lon"]) / meta["pixel_size_lon"] + 1e-6) + return row, col + + +def _pixel_to_latlon(row, col, meta): + lat = meta["origin_lat"] + row * meta["pixel_size_lat"] + lon = meta["origin_lon"] + col * meta["pixel_size_lon"] + return lat, lon + + +def _bidir(lat, lon, pairs, cost_s): + """Expand each bidirectional m↔m' pair into two directed (lat, lon, from, to, cost) tuples (§4).""" + out = [] + for a, b in pairs: + ia, ib = MODE_INDEX[a], MODE_INDEX[b] + out.append((lat, lon, ia, ib, cost_s)) + out.append((lat, lon, ib, ia, cost_s)) + return out + + +def _bearing(p1, l1, p2, l2): + return math.atan2(math.sin(l2 - l1) * math.cos(p2), + math.cos(p1) * math.sin(p2) - math.sin(p1) * math.cos(p2) * math.cos(l2 - l1)) + + +def _cross_track_distance_m(lat, lon, line): + """Great-circle perpendicular distance (m) from a point to the line through the two + endpoints (spec §5). Falls back to point distance for a degenerate line.""" + (lat1, lon1), (lat2, lon2) = line + p1, l1 = math.radians(lat1), math.radians(lon1) + p3, l3 = math.radians(lat), math.radians(lon) + h = math.sin((p3 - p1) / 2) ** 2 + math.cos(p1) * math.cos(p3) * math.sin((l3 - l1) / 2) ** 2 + d13 = 2 * math.asin(min(1.0, math.sqrt(h))) # haversine angle, start->point + if lat1 == lat2 and lon1 == lon2: + return d13 * _EARTH_R_M + dth = _bearing(p1, l1, p3, l3) - _bearing(p1, l1, math.radians(lat2), math.radians(lon2)) + return abs(math.asin(max(-1.0, min(1.0, math.sin(d13) * math.sin(dth))))) * _EARTH_R_M + + +def _cap_candidates(raw, line): + """§5 cap for one transition type: group by (lat, lon) so a point's several directed + tuples count as ONE candidate, keep the closest _CAP_PER_TYPE points within + _CAP_RADIUS_M of `line`, flatten. line=None -> uncapped (test convenience).""" + if not raw: + return [] + if line is None: + return list(raw) + groups = {} + for t in raw: + groups.setdefault((t[0], t[1]), []).append(t) + scored = [] + for (lat, lon), tuples in groups.items(): + d = _cross_track_distance_m(lat, lon, line) + if d <= _CAP_RADIUS_M: + scored.append((d, tuples)) + scored.sort(key=lambda x: x[0]) + out = [] + for _d, tuples in scored[:_CAP_PER_TYPE]: + out.extend(tuples) + return out + + +def parking_transitions_near_line(line, buffer_m=5000): + """Parking mode switches near the line (§4): foot↔{vehicle,4w,2w} at + TRANSITION_COST_PARKING_S. Blocked-access lots skipped.""" + coords = [tuple(line[0]), tuple(line[1])] + index = load_parking_index() + pairs = (("foot", "vehicle"), ("foot", "4w"), ("foot", "2w")) + out = [] + for rec in index.query_parking_near_line(coords, buffer_m): + if rec.get("access") in _BLOCKED_ACCESS: + continue + out.extend(_bidir(rec["lat"], rec["lon"], pairs, TRANSITION_COST_PARKING_S)) + return out + + +def trailhead_transitions_near_line(line, buffer_m=5000): + """Trailhead mode switches near the line (§4): foot↔4w, foot↔2w at + TRANSITION_COST_TRAILHEAD_S (no full-size vehicle — the tow vehicle stays parked).""" + coords = [tuple(line[0]), tuple(line[1])] + index = load_trailheads() + pairs = (("foot", "4w"), ("foot", "2w")) + out = [] + for rec in index.query_trailheads_near_line(coords, buffer_m): + out.extend(_bidir(rec["lat"], rec["lon"], pairs, TRANSITION_COST_TRAILHEAD_S)) + return out + + +def road_terminus_transitions(meta, trail_grid, elevation=None): + """Road-terminus mode switches from the trail raster (spec §4): a road(5)/track(15) cell + with a passable off-network 8-neighbour (value 0; finite elev when `elevation` given). + foot↔vehicle at TRANSITION_COST_ROAD_TERMINUS_S. Pure raster scan, no DB — fixes §1.""" + rows, cols = trail_grid.shape + pairs = (("foot", "vehicle"),) + road = (trail_grid == 5) | (trail_grid == 15) + rs, cs = np.nonzero(road) + out = [] + for r, c in zip(rs.tolist(), cs.tolist()): + is_terminus = False + for dr in (-1, 0, 1): + for dc in (-1, 0, 1): + if dr == 0 and dc == 0: + continue + nr, nc = r + dr, c + dc + if nr < 0 or nr >= rows or nc < 0 or nc >= cols: + continue + if trail_grid[nr, nc] != 0: + continue + if elevation is not None and not np.isfinite(elevation[nr, nc]): + continue + is_terminus = True + break + if is_terminus: + break + if is_terminus: + lat, lon = _pixel_to_latlon(r, c, meta) + out.extend(_bidir(lat, lon, pairs, TRANSITION_COST_ROAD_TERMINUS_S)) + return out + + +def surface_change_transitions_near_line(line, valhalla_url, buffer_m=5000): + """Surface-change mode switches along the line (§4) at TRANSITION_COST_SURFACE_CHANGE_S + (free). The candidate record encodes no mode info, so the default wheeled swaps + vehicle↔4w and 4w↔2w are used. (buffer_m accepted for signature parity.)""" + coords = [tuple(line[0]), tuple(line[1])] + pairs = (("vehicle", "4w"), ("4w", "2w")) + out = [] + for rec in get_surface_change_candidates(coords, valhalla_url): + out.extend(_bidir(rec["lat"], rec["lon"], pairs, TRANSITION_COST_SURFACE_CHANGE_S)) + return out + + +def gather_transition_cells(meta, endpoint_line=None, trail_grid=None, + elevation=None, valhalla_url=None, buffer_m=5000): + """All transition cells for one Auto search (spec §4–§5). Sources the four types, + caps each independently (closest 15 within 5 km of `endpoint_line`), maps lat/lon → + grid pixel via `meta`, drops out-of-bounds, de-dupes per (row, col, from, to), and + returns the flat directed list. Sources lacking their input are skipped: the + line-based ones need `endpoint_line`, road-terminus needs `trail_grid`, surface + additionally needs `valhalla_url`. endpoint_line=None -> uncapped (test convenience). + """ + rows, cols = meta["shape"] + per_type = [] + if endpoint_line is not None: + per_type.append(_cap_candidates( + parking_transitions_near_line(endpoint_line, buffer_m), endpoint_line)) + per_type.append(_cap_candidates( + trailhead_transitions_near_line(endpoint_line, buffer_m), endpoint_line)) + if trail_grid is not None: + per_type.append(_cap_candidates( + road_terminus_transitions(meta, trail_grid, elevation), endpoint_line)) + if endpoint_line is not None and valhalla_url: + per_type.append(_cap_candidates( + surface_change_transitions_near_line(endpoint_line, valhalla_url, buffer_m), + endpoint_line)) + + seen = set() + out = [] + for raw in per_type: + for (lat, lon, from_m, to_m, cost_s) in raw: + row, col = _latlon_to_pixel(lat, lon, meta) + if not (0 <= row < rows and 0 <= col < cols): + continue + key = (row, col, from_m, to_m) + if key in seen: + continue + seen.add(key) + out.append((row, col, from_m, to_m, cost_s)) + return out