navi-offroute: unified cost layers + transition cells (Phase 3) (#39)

Co-authored-by: mj <mj@k7zvx.com>
Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

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)
 # ═══════════════════════════════════════════════════════════════════════════════

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"

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