mirror of
https://github.com/zvx-echo6/navi.git
synced 2026-08-26 17:31:37 +00:00
navi-offroute: rewire _route_auto to unified A* (Phase 4)
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
parent
2ac005ab16
commit
28e41ae019
4 changed files with 551 additions and 126 deletions
|
|
@ -375,14 +375,38 @@ def astar_multigoal_multimode(
|
||||||
for gi in range(goal_modes.shape[0]):
|
for gi in range(goal_modes.shape[0]):
|
||||||
goal_mode_ok[goal_modes[gi]] = True
|
goal_mode_ok[goal_modes[gi]] = True
|
||||||
|
|
||||||
# §10: divide straight-line distance by the FASTEST base speed over goal modes
|
# §10: divide straight-line distance by the FASTEST EFFECTIVE speed over goal modes
|
||||||
# -> smallest possible finishing time -> admissible lower bound. Independent of
|
# -> smallest possible finishing time -> admissible lower bound. Independent of the
|
||||||
# the state's current mode (a slow-mode state may switch to a fast mode later).
|
# state's current mode (a slow-mode state may switch to a fast mode later).
|
||||||
max_goal_speed = 0.0
|
max_goal_speed = 0.0
|
||||||
for gi in range(goal_modes.shape[0]):
|
for gi in range(goal_modes.shape[0]):
|
||||||
s = base_speed_kmh_arr[goal_modes[gi]]
|
s = base_speed_kmh_arr[goal_modes[gi]]
|
||||||
if s > max_goal_speed:
|
if s > max_goal_speed:
|
||||||
max_goal_speed = s
|
max_goal_speed = s
|
||||||
|
# An edge's time is base_time * factor, where the factor is the trail friction (on a
|
||||||
|
# trail) or the avg context multiplier (off-trail); effective speed = base / factor. So
|
||||||
|
# distance/base_speed alone OVERESTIMATES remaining cost wherever some factor < 1.0 (a
|
||||||
|
# road's 0.1 friction is ~10x faster) -> inadmissible. Bound instead by base speed /
|
||||||
|
# the SMALLEST factor reachable by any goal mode, over BOTH trail friction and the
|
||||||
|
# context multiplier (network_affinity can scale trail friction above the off-trail
|
||||||
|
# multiplier, so trail friction alone is not always the fastest surface).
|
||||||
|
min_factor = INF
|
||||||
|
for gi in range(goal_modes.shape[0]):
|
||||||
|
gm = goal_modes[gi]
|
||||||
|
for v in range(256):
|
||||||
|
f = trail_friction_stack[gm, v]
|
||||||
|
if f < min_factor:
|
||||||
|
min_factor = f
|
||||||
|
for rr in range(rows):
|
||||||
|
for cc in range(cols):
|
||||||
|
cmv = cost_mult_stack[rr, cc, gm]
|
||||||
|
if cmv < min_factor:
|
||||||
|
min_factor = cmv
|
||||||
|
if not (min_factor < INF):
|
||||||
|
min_factor = 1.0
|
||||||
|
if min_factor < 1e-6:
|
||||||
|
min_factor = 1e-6
|
||||||
|
max_effective_speed = max_goal_speed / min_factor
|
||||||
|
|
||||||
g_score = np.full((rows, cols, n_modes), INF, dtype=np.float64)
|
g_score = np.full((rows, cols, n_modes), INF, dtype=np.float64)
|
||||||
parent = np.full((rows, cols, n_modes), -1, dtype=np.int64) # parent's packed heap id
|
parent = np.full((rows, cols, n_modes), -1, dtype=np.int64) # parent's packed heap id
|
||||||
|
|
@ -435,7 +459,7 @@ def astar_multigoal_multimode(
|
||||||
d = math.sqrt(dr * dr + dc * dc)
|
d = math.sqrt(dr * dr + dc * dc)
|
||||||
if d < best:
|
if d < best:
|
||||||
best = d
|
best = d
|
||||||
return best * 3.6 / max_goal_speed # metres -> seconds at fastest goal speed
|
return best * 3.6 / max_effective_speed # metres -> s at fastest effective speed (§10)
|
||||||
|
|
||||||
# Seed every allowed origin mode at the origin cell.
|
# Seed every allowed origin mode at the origin cell.
|
||||||
for oi in range(origin_modes.shape[0]):
|
for oi in range(origin_modes.shape[0]):
|
||||||
|
|
|
||||||
|
|
@ -526,24 +526,34 @@ def compute_unified_cost_layers(
|
||||||
boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic",
|
boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic",
|
||||||
endpoint_line=None,
|
endpoint_line=None,
|
||||||
valhalla_url=None,
|
valhalla_url=None,
|
||||||
|
mvum_by_mode: Optional[Dict[str, np.ndarray]] = None,
|
||||||
|
network_affinity: Optional[Dict[str, float]] = None,
|
||||||
) -> dict:
|
) -> dict:
|
||||||
"""Per-mode inflated cost layers + mode-transition cells for one Auto search
|
"""Per-mode inflated cost layers + mode-transition cells for one Auto search
|
||||||
(spec §3.2 / §4 / §5). Returns {"cost_mult": {mode: ndarray}, "transition_cells":
|
(spec §3.2 / §4 / §5 / §8). Returns {"cost_mult": {mode: ndarray}, "transition_cells":
|
||||||
[(row, col, from_idx, to_idx, cost_s), ...], "meta": {...DEMReader meta, +
|
[(row, col, from_idx, to_idx, cost_s), ...], "meta": {...DEMReader meta, +
|
||||||
"boundary_mode"}}.
|
"boundary_mode"}}.
|
||||||
|
|
||||||
Rasters are INJECTED, not fetched: the raster IO lives on the router's reader
|
Rasters are INJECTED, not fetched: the raster IO lives on the router's reader objects
|
||||||
objects (router.py::_pathfind_wilderness) and is not duplicated — Phase 4 passes
|
(router.py::_pathfind_wilderness) and is not duplicated — Phase 4 passes the rasters +
|
||||||
elevation/friction/trails/wilderness + DEMReader `meta` straight in; tests pass
|
DEMReader `meta` straight in; tests pass synthetic arrays. Each mode's multiplier comes
|
||||||
synthetic arrays. Each mode's multiplier comes from compute_cost_multiplier_grid(...)
|
from compute_cost_multiplier_grid(...), then (still pre-inflation) network_affinity (§8)
|
||||||
then inflate_cost_multiplier(...). boundary_mode governs barrier/MVUM rules, which
|
and MVUM closures (§9) are applied, then inflate_cost_multiplier(...).
|
||||||
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).
|
mvum_by_mode: optional {mode: uint8[rows,cols]} (1=open, 255=closed, 0=unknown). For
|
||||||
|
each MOTORIZED mode, closed cells become impassable under `strict`, ×PRAGMATIC under
|
||||||
|
`pragmatic`, ignored under `emergency`; foot skips MVUM entirely. network_affinity:
|
||||||
|
optional {mode: float} multiplying that mode's on-network cells (trails != 0); default
|
||||||
|
1.0 is a no-op. boundary_mode also governs barrier rules, which stay PER-EDGE in the
|
||||||
|
kernel — threaded into the returned meta for Phase 4 (mvum_by_mode=network_affinity=None
|
||||||
|
reproduces the Phase-3 layers exactly, keeping the parity test green).
|
||||||
"""
|
"""
|
||||||
from .astar import inflate_cost_multiplier
|
from .astar import inflate_cost_multiplier
|
||||||
from .transitions import gather_transition_cells
|
from .transitions import gather_transition_cells
|
||||||
|
|
||||||
cell_size_m = float(meta["cell_size_m"])
|
cell_size_m = float(meta["cell_size_m"])
|
||||||
|
network_affinity = network_affinity or {}
|
||||||
|
on_network = (trails != 0) if trails is not None else None
|
||||||
cost_mult = {}
|
cost_mult = {}
|
||||||
for mode in modes:
|
for mode in modes:
|
||||||
m = compute_cost_multiplier_grid(
|
m = compute_cost_multiplier_grid(
|
||||||
|
|
@ -555,7 +565,26 @@ def compute_unified_cost_layers(
|
||||||
wilderness=wilderness,
|
wilderness=wilderness,
|
||||||
mode=mode,
|
mode=mode,
|
||||||
)
|
)
|
||||||
cost_mult[mode] = inflate_cost_multiplier(m)
|
# §9 MVUM closures: motorized modes only, pre-inflation (closures must bleed like any
|
||||||
|
# impassable cell), modulated by boundary_mode.
|
||||||
|
if mvum_by_mode is not None and mode != "foot" and boundary_mode != "emergency":
|
||||||
|
mv = mvum_by_mode.get(mode)
|
||||||
|
if mv is not None:
|
||||||
|
closed = mv == 255
|
||||||
|
if boundary_mode == "strict":
|
||||||
|
m[closed] = np.inf
|
||||||
|
else: # pragmatic
|
||||||
|
m[closed & np.isfinite(m)] *= PRAGMATIC_BARRIER_MULTIPLIER
|
||||||
|
inflated = inflate_cost_multiplier(m)
|
||||||
|
# §8 network_affinity, applied AFTER inflation: a >1 penalty on on-network cells must
|
||||||
|
# NOT bleed into off-network neighbours (pre-inflation it makes LEAVING the network
|
||||||
|
# harder -- the opposite of intent). The kernel costs on-network EDGES from
|
||||||
|
# trail_friction (not cost_mult), so this is a no-op for movement today; the functional
|
||||||
|
# bias is applied to trail_friction in router packing. Kept here per spec §8.
|
||||||
|
aff = float(network_affinity.get(mode, 1.0))
|
||||||
|
if aff != 1.0 and on_network is not None:
|
||||||
|
inflated[on_network & np.isfinite(inflated)] *= aff
|
||||||
|
cost_mult[mode] = inflated
|
||||||
|
|
||||||
transition_cells = gather_transition_cells(
|
transition_cells = gather_transition_cells(
|
||||||
meta,
|
meta,
|
||||||
|
|
|
||||||
|
|
@ -33,12 +33,14 @@ import requests
|
||||||
import psycopg2
|
import psycopg2
|
||||||
import psycopg2.extras
|
import psycopg2.extras
|
||||||
from shapely.geometry import LineString, Point
|
from shapely.geometry import LineString, Point
|
||||||
from .astar import astar_multigoal, inflate_cost_multiplier
|
from .astar import astar_multigoal, astar_multigoal_multimode, inflate_cost_multiplier
|
||||||
from .mvum_surface_change import get_surface_change_candidates
|
from .mvum_surface_change import get_surface_change_candidates
|
||||||
from .mvum_parking import load_parking_index # noqa: F401 (singleton injected by handler)
|
from .mvum_parking import load_parking_index # noqa: F401 (singleton injected by handler)
|
||||||
|
|
||||||
from shared.dem import DEMReader, dem_path
|
from shared.dem import DEMReader, dem_path
|
||||||
from .cost import compute_cost_grid, compute_cost_multiplier_grid, MODE_PROFILES
|
from .cost import (compute_cost_grid, compute_cost_multiplier_grid, MODE_PROFILES,
|
||||||
|
compute_unified_cost_layers)
|
||||||
|
from .transitions import MODE_INDEX
|
||||||
from .friction import FrictionReader, friction_to_multiplier
|
from .friction import FrictionReader, friction_to_multiplier
|
||||||
from .barriers import BarrierReader, WildernessReader, wilderness_tif_path
|
from .barriers import BarrierReader, WildernessReader, wilderness_tif_path
|
||||||
from .trails import TrailReader
|
from .trails import TrailReader
|
||||||
|
|
@ -100,6 +102,10 @@ MODE_TO_COSTING = {
|
||||||
# demanding terrain) and uses the first that yields a usable route.
|
# demanding terrain) and uses the first that yields a usable route.
|
||||||
AUTO_MODE_PRIORITY = ["vehicle", "4w", "2w", "foot"]
|
AUTO_MODE_PRIORITY = ["vehicle", "4w", "2w", "foot"]
|
||||||
|
|
||||||
|
# Fixed mode index ordering for the unified-graph kernel (spec §2.1; == MODE_INDEX in
|
||||||
|
# transitions.py). The cost_mult_stack / per-mode arrays are packed in this order.
|
||||||
|
MODE_ORDER = ["foot", "2w", "4w", "vehicle"]
|
||||||
|
|
||||||
# MVUM Layer 3a: implicit multi-modal Auto. On long trips, "drive in to a trailhead,
|
# MVUM Layer 3a: implicit multi-modal Auto. On long trips, "drive in to a trailhead,
|
||||||
# switch vehicles, continue offroad" can beat the single-mode winner; Auto picks that
|
# switch vehicles, continue offroad" can beat the single-mode winner; Auto picks that
|
||||||
# hybrid plan when it does. Leg times are summed with NO transition penalty, and the
|
# hybrid plan when it does. Leg times are summed with NO transition penalty, and the
|
||||||
|
|
@ -629,6 +635,7 @@ class OffrouteRouter:
|
||||||
start_category: Optional[str] = None,
|
start_category: Optional[str] = None,
|
||||||
end_category: Optional[str] = None,
|
end_category: Optional[str] = None,
|
||||||
annotate_mvum: bool = True,
|
annotate_mvum: bool = True,
|
||||||
|
network_affinity: Optional[Dict[str, float]] = None,
|
||||||
) -> Dict:
|
) -> Dict:
|
||||||
"""
|
"""
|
||||||
Route between two points, handling all four scenarios.
|
Route between two points, handling all four scenarios.
|
||||||
|
|
@ -650,7 +657,7 @@ class OffrouteRouter:
|
||||||
if mode == "auto":
|
if mode == "auto":
|
||||||
return self._route_auto(
|
return self._route_auto(
|
||||||
start_lat, start_lon, end_lat, end_lon, boundary_mode,
|
start_lat, start_lon, end_lat, end_lon, boundary_mode,
|
||||||
start_category, end_category
|
start_category, end_category, network_affinity
|
||||||
)
|
)
|
||||||
|
|
||||||
if mode not in MODE_TO_COSTING:
|
if mode not in MODE_TO_COSTING:
|
||||||
|
|
@ -772,7 +779,13 @@ class OffrouteRouter:
|
||||||
"""Eligible modes for an UNTYPED endpoint, derived from Valhalla /locate snaps.
|
"""Eligible modes for an UNTYPED endpoint, derived from Valhalla /locate snaps.
|
||||||
Runs the three distinct costings (auto/pedestrian/bicycle) in parallel and
|
Runs the three distinct costings (auto/pedestrian/bicycle) in parallel and
|
||||||
applies the per-mode snap-distance + road-class rules. snap_cache dedupes
|
applies the per-mode snap-distance + road-class rules. snap_cache dedupes
|
||||||
/locate results within a single request."""
|
/locate results within a single request.
|
||||||
|
|
||||||
|
Spec §6 reframe: under unified-graph Auto this is now a SEED generator (it
|
||||||
|
feeds origin/goal modes to astar_multigoal_multimode), not the load-bearing
|
||||||
|
single-mode chooser it was. Behaviour/return shape are unchanged. The §6
|
||||||
|
single-combined-/locate batch is a Phase-4 follow-up; the 3-call pattern stays
|
||||||
|
until the batch response shape is confirmed against the live instance."""
|
||||||
# auto costing -> vehicle/4w reach, bicycle -> 2w reach, pedestrian -> foot
|
# auto costing -> vehicle/4w reach, bicycle -> 2w reach, pedestrian -> foot
|
||||||
costing_modes = {"auto": "vehicle", "pedestrian": "foot", "bicycle": "2w"}
|
costing_modes = {"auto": "vehicle", "pedestrian": "foot", "bicycle": "2w"}
|
||||||
need = [c for c in costing_modes if (lat, lon, c) not in snap_cache]
|
need = [c for c in costing_modes if (lat, lon, c) not in snap_cache]
|
||||||
|
|
@ -813,118 +826,245 @@ class OffrouteRouter:
|
||||||
end_lat: float, end_lon: float,
|
end_lat: float, end_lon: float,
|
||||||
boundary_mode: str,
|
boundary_mode: str,
|
||||||
start_category: Optional[str] = None,
|
start_category: Optional[str] = None,
|
||||||
end_category: Optional[str] = None
|
end_category: Optional[str] = None,
|
||||||
|
network_affinity: Optional[Dict[str, float]] = None,
|
||||||
) -> Dict:
|
) -> Dict:
|
||||||
"""
|
"""Unified-graph Auto (spec §2.4): ONE A* over (row, col, mode). Endpoint
|
||||||
Auto mode: per-endpoint eligible-mode-set intersection.
|
eligibility only SEEDS the start/goal modes (§6); the optimizer decides where to
|
||||||
|
switch modes (parking / trailhead / road terminus / surface change) instead of
|
||||||
|
committing to a single trip-wide mode. Supersedes the capability pick + single
|
||||||
|
self.route() + _try_hybrid_auto + foot-fallback flow (those stay in place but are
|
||||||
|
no longer reached from here; Phase 5 removes them). No auto_fallback_from (§7)."""
|
||||||
|
# 1. Bootstrap eligibility -> seeds (not a routing decision).
|
||||||
|
snap_cache: dict = {}
|
||||||
|
start_eligible = self._auto_eligible_modes(start_lat, start_lon, start_category, snap_cache)
|
||||||
|
end_eligible = self._auto_eligible_modes(end_lat, end_lon, end_category, snap_cache)
|
||||||
|
seed_set = sorted(start_eligible | end_eligible)
|
||||||
|
|
||||||
Each endpoint's eligible modes come from its category type-hint
|
# 2-3. bbox covering both endpoints + the shared rasters (one fetch, all modes).
|
||||||
(CATEGORY_ELIGIBLE_MODES); an untyped endpoint falls back to a spatial
|
try:
|
||||||
Valhalla-snap probe. Auto probes the intersection of both endpoints'
|
(elevation, friction_mult, friction_raw, trails, barriers,
|
||||||
eligible sets and returns the candidate that minimises total trip time.
|
wilderness, mvum_by_mode, meta) = self._fetch_auto_rasters(
|
||||||
selected_mode + selected_mode_set are added for visibility.
|
start_lat, start_lon, end_lat, end_lon)
|
||||||
"""
|
except Exception as e:
|
||||||
snap_cache = {}
|
logger.exception("auto: raster fetch failed")
|
||||||
start_typed = self._eligible_modes_from_category(start_category)
|
return {"status": "error", "message": f"Failed to load terrain: {e}",
|
||||||
end_typed = self._eligible_modes_from_category(end_category)
|
"selected_mode_set": seed_set}
|
||||||
|
rows, cols = elevation.shape
|
||||||
|
|
||||||
jobs = {}
|
# Endpoints -> pixels (mirror the existing entry-point convention).
|
||||||
if start_typed is None:
|
origin_row, origin_col = self.dem_reader.latlon_to_pixel(start_lat, start_lon, meta)
|
||||||
jobs["start"] = (start_lat, start_lon)
|
goal_row, goal_col = self.dem_reader.latlon_to_pixel(end_lat, end_lon, meta)
|
||||||
if end_typed is None:
|
if not (0 <= origin_row < rows and 0 <= origin_col < cols
|
||||||
jobs["end"] = (end_lat, end_lon)
|
and 0 <= goal_row < rows and 0 <= goal_col < cols):
|
||||||
|
return {"status": "error", "message": "Endpoint outside grid bounds",
|
||||||
|
"selected_mode_set": seed_set}
|
||||||
|
|
||||||
spatial = {}
|
# 4. Per-mode cost layers (+ MVUM closures + network_affinity) and transition cells.
|
||||||
if len(jobs) == 2:
|
layers = compute_unified_cost_layers(
|
||||||
# Both endpoints untyped: resolve them in parallel.
|
elevation, friction_mult, friction_raw, trails, wilderness, meta,
|
||||||
with ThreadPoolExecutor(max_workers=2) as ex:
|
modes=tuple(MODE_ORDER), boundary_mode=boundary_mode,
|
||||||
futs = {ex.submit(self._spatial_eligible_modes, la, lo, snap_cache): name
|
endpoint_line=((start_lat, start_lon), (end_lat, end_lon)),
|
||||||
for name, (la, lo) in jobs.items()}
|
valhalla_url=VALHALLA_URL,
|
||||||
for fut in as_completed(futs):
|
mvum_by_mode=mvum_by_mode, network_affinity=network_affinity)
|
||||||
spatial[futs[fut]] = fut.result()
|
|
||||||
else:
|
|
||||||
for name, (la, lo) in jobs.items():
|
|
||||||
spatial[name] = self._spatial_eligible_modes(la, lo, snap_cache)
|
|
||||||
|
|
||||||
start_eligible = start_typed if start_typed is not None else spatial["start"]
|
# 5-6. Pack the per-mode arrays the kernel expects (MODE_ORDER == MODE_INDEX order).
|
||||||
end_eligible = end_typed if end_typed is not None else spatial["end"]
|
n_modes = len(MODE_ORDER)
|
||||||
|
cost_mult = layers["cost_mult"]
|
||||||
|
cost_mult_stack = np.empty((rows, cols, n_modes), dtype=np.float64)
|
||||||
|
trail_friction_stack = np.full((n_modes, 256), np.inf, dtype=np.float64)
|
||||||
|
max_grade_arr = np.empty(n_modes, dtype=np.float64)
|
||||||
|
speed_function_ids = np.empty(n_modes, dtype=np.int64)
|
||||||
|
base_speed_kmh_arr = np.empty(n_modes, dtype=np.float64)
|
||||||
|
sf_id = {"tobler": 0, "herzog": 1, "linear": 2}
|
||||||
|
net_aff = network_affinity or {}
|
||||||
|
for mi, mname in enumerate(MODE_ORDER):
|
||||||
|
cost_mult_stack[:, :, mi] = cost_mult[mname]
|
||||||
|
prof = MODE_PROFILES[mname]
|
||||||
|
# §8 network_affinity also scales this mode's on-network (trail) edges: the
|
||||||
|
# kernel costs on-trail edges from trail_friction, NOT cost_mult, so the cost_mult
|
||||||
|
# bias in compute_unified_cost_layers only reaches off-network cells. >1 penalizes
|
||||||
|
# staying on the network, <1 biases toward it; default 1.0 is a no-op.
|
||||||
|
aff = float(net_aff.get(mname, 1.0))
|
||||||
|
for tv, fric in prof.trail_friction.items():
|
||||||
|
trail_friction_stack[mi, tv] = np.inf if fric is None else float(fric) * aff
|
||||||
|
max_grade_arr[mi] = float(np.tan(np.radians(prof.max_slope_deg)))
|
||||||
|
speed_function_ids[mi] = sf_id.get(prof.speed_function, 0)
|
||||||
|
base_speed_kmh_arr[mi] = float(prof.base_speed_kmh)
|
||||||
|
|
||||||
intersection = start_eligible & end_eligible
|
origin_modes = np.array(sorted(MODE_INDEX[m] for m in start_eligible), dtype=np.int64)
|
||||||
if not intersection:
|
goal_modes = np.array(sorted(MODE_INDEX[m] for m in end_eligible), dtype=np.int64)
|
||||||
# foot is always eligible, so this is defensive only.
|
|
||||||
intersection = frozenset({"foot"})
|
|
||||||
mode_set = sorted(intersection)
|
|
||||||
|
|
||||||
priority = [m for m in AUTO_MODE_PRIORITY if m in intersection]
|
# 7. Unpack transition cells into the kernel's flat 1D arrays.
|
||||||
|
tc = layers["transition_cells"]
|
||||||
|
nt = len(tc)
|
||||||
|
trans_rows = np.empty(nt, dtype=np.int64)
|
||||||
|
trans_cols = np.empty(nt, dtype=np.int64)
|
||||||
|
trans_from = np.empty(nt, dtype=np.int64)
|
||||||
|
trans_to = np.empty(nt, dtype=np.int64)
|
||||||
|
trans_cost = np.empty(nt, dtype=np.float64)
|
||||||
|
for i, (tr, tcl, fm, tm, cs) in enumerate(tc):
|
||||||
|
trans_rows[i], trans_cols[i], trans_from[i], trans_to[i], trans_cost[i] = tr, tcl, fm, tm, cs
|
||||||
|
|
||||||
# Classify-once / route-once: the eligible-mode sets above already identify the
|
# 8. Single unified search.
|
||||||
# fastest mode both endpoints can traverse -- the first in AUTO_MODE_PRIORITY
|
cell_m = float(meta["cell_size_m"])
|
||||||
# that survives the intersection. Pick it and route a SINGLE time, instead of
|
trail_grid = np.ascontiguousarray(
|
||||||
# routing all four candidates and keeping a min-time winner (the old 4-mode
|
trails if trails is not None else np.zeros((rows, cols), np.uint8), dtype=np.uint8)
|
||||||
# contest cost ~3s on in-town trips). No routing-failure fall-through: if the
|
barrier_grid = np.ascontiguousarray(
|
||||||
# picked mode cannot route, the error is returned. See auto-rewrite-plan.md.
|
barriers if barriers is not None else np.zeros((rows, cols), np.uint8), dtype=np.uint8)
|
||||||
mode = priority[0] if priority else "foot"
|
boundary_mode_id = {"strict": 0, "pragmatic": 1, "emergency": 2}.get(boundary_mode, 1)
|
||||||
best_result = None
|
_t0 = time.perf_counter()
|
||||||
best_minutes = None
|
best_idx, path, total_cost = astar_multigoal_multimode(
|
||||||
last_error = None
|
np.ascontiguousarray(cost_mult_stack), elevation, cell_m, cell_m,
|
||||||
_probe_t0 = time.perf_counter()
|
max_grade_arr, speed_function_ids, base_speed_kmh_arr,
|
||||||
result = self.route(
|
trail_grid, trail_friction_stack, barrier_grid, boundary_mode_id,
|
||||||
start_lat, start_lon, end_lat, end_lon,
|
int(origin_row), int(origin_col), origin_modes,
|
||||||
mode=mode, boundary_mode=boundary_mode, annotate_mvum=False
|
np.array([goal_row], dtype=np.int64), np.array([goal_col], dtype=np.int64), goal_modes,
|
||||||
)
|
trans_rows, trans_cols, trans_from, trans_to, trans_cost)
|
||||||
if result.get("status") == "ok":
|
logger.info("auto: unified A* (%d transition cells) in %.2fs",
|
||||||
best_result = result
|
nt, time.perf_counter() - _t0)
|
||||||
best_result["selected_mode"] = mode
|
|
||||||
best_minutes = (result.get("summary") or {}).get(
|
|
||||||
"total_effort_minutes", float("inf"))
|
|
||||||
else:
|
|
||||||
last_error = result
|
|
||||||
logger.info("auto: classified mode=%s, routed once in %.2fs",
|
|
||||||
mode, time.perf_counter() - _probe_t0)
|
|
||||||
|
|
||||||
# Foot-as-last-resort: foot always routes (modulo bbox limits), so if the
|
if best_idx < 0 or path.shape[0] == 0:
|
||||||
# capability-picked mode failed, fall back to foot ONCE rather than surface a
|
return {"status": "error", "message": "No unified route found",
|
||||||
# wall to the user. selected_mode_set still reflects the original eligibility.
|
"selected_mode_set": seed_set}
|
||||||
if result.get("status") != "ok" and mode != "foot":
|
|
||||||
_foot_t0 = time.perf_counter()
|
|
||||||
foot_result = self.route(
|
|
||||||
start_lat, start_lon, end_lat, end_lon,
|
|
||||||
mode="foot", boundary_mode=boundary_mode, annotate_mvum=False
|
|
||||||
)
|
|
||||||
if foot_result.get("status") == "ok":
|
|
||||||
best_result = foot_result
|
|
||||||
best_result["selected_mode"] = "foot"
|
|
||||||
best_result["auto_fallback_from"] = mode # surface to client/UI
|
|
||||||
best_minutes = (foot_result.get("summary") or {}).get(
|
|
||||||
"total_effort_minutes", float("inf"))
|
|
||||||
last_error = None
|
|
||||||
logger.info("auto: %s failed, foot fallback succeeded in %.2fs",
|
|
||||||
mode, time.perf_counter() - _foot_t0)
|
|
||||||
else:
|
|
||||||
# foot also failed -- keep the original last_error (return original error)
|
|
||||||
logger.info("auto: %s failed, foot fallback also failed in %.2fs",
|
|
||||||
mode, time.perf_counter() - _foot_t0)
|
|
||||||
|
|
||||||
if best_result is not None:
|
# 9. Render per-mode segments + transition markers (no auto_fallback_from, §7).
|
||||||
# MVUM Layer 3a: a "drive to a trailhead, switch, continue offroad" plan may
|
return self._render_unified_path(path, total_cost, meta, boundary_mode)
|
||||||
# beat the single-mode winner on long trips. If so, return it instead.
|
|
||||||
hybrid = self._try_hybrid_auto(
|
|
||||||
start_lat, start_lon, end_lat, end_lon, boundary_mode,
|
|
||||||
best_result, best_minutes, intersection)
|
|
||||||
if hybrid is not None:
|
|
||||||
hybrid["selected_mode_set"] = mode_set
|
|
||||||
return hybrid
|
|
||||||
best_result["selected_mode_set"] = mode_set
|
|
||||||
self._annotate_network_segments(best_result, best_result["selected_mode"])
|
|
||||||
return best_result
|
|
||||||
|
|
||||||
if last_error is not None:
|
def _auto_eligible_modes(self, lat, lon, category, snap_cache):
|
||||||
last_error["selected_mode_set"] = mode_set
|
"""Seed modes for one endpoint: category type-hint when present, else the
|
||||||
return last_error
|
reframed spatial probe (§6). foot is always eligible."""
|
||||||
|
typed = self._eligible_modes_from_category(category)
|
||||||
|
if typed is not None:
|
||||||
|
return frozenset(typed) | {"foot"}
|
||||||
|
return frozenset(self._spatial_eligible_modes(lat, lon, snap_cache)) | {"foot"}
|
||||||
|
|
||||||
|
def _fetch_auto_rasters(self, start_lat, start_lon, end_lat, end_lon):
|
||||||
|
"""Fetch the shared rasters for one unified Auto search over a bbox covering both
|
||||||
|
endpoints (+ pad, clamped), via the existing reader objects. Returns
|
||||||
|
(elevation, friction_mult, friction_raw, trails, barriers, wilderness,
|
||||||
|
mvum_by_mode, meta)."""
|
||||||
|
self._init_readers()
|
||||||
|
pad = 0.02
|
||||||
|
bbox = {"south": min(start_lat, end_lat) - pad, "north": max(start_lat, end_lat) + pad,
|
||||||
|
"west": min(start_lon, end_lon) - pad, "east": max(start_lon, end_lon) + pad}
|
||||||
|
MAX = 2.0
|
||||||
|
if (bbox["north"] - bbox["south"] > MAX) or (bbox["east"] - bbox["west"] > MAX):
|
||||||
|
clat, clon, h = (start_lat + end_lat) / 2, (start_lon + end_lon) / 2, MAX / 2
|
||||||
|
bbox = {"south": clat - h, "north": clat + h, "west": clon - h, "east": clon + h}
|
||||||
|
elevation, meta = self.dem_reader.get_elevation_grid(
|
||||||
|
south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"])
|
||||||
|
shape = elevation.shape
|
||||||
|
friction_raw = self.friction_reader.get_friction_grid(
|
||||||
|
south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"],
|
||||||
|
target_shape=shape)
|
||||||
|
friction_mult = friction_to_multiplier(friction_raw)
|
||||||
|
barriers = self.barrier_reader.get_barrier_grid(
|
||||||
|
south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"],
|
||||||
|
target_shape=shape)
|
||||||
|
trails = self.trail_reader.get_trails_grid(
|
||||||
|
south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"],
|
||||||
|
target_shape=shape)
|
||||||
|
wilderness = None
|
||||||
|
if self.wilderness_reader is not None:
|
||||||
|
wilderness = self.wilderness_reader.get_wilderness_grid(
|
||||||
|
south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"],
|
||||||
|
target_shape=shape)
|
||||||
|
mvum_by_mode = self._build_mvum_by_mode(bbox, shape)
|
||||||
|
elevation = np.ascontiguousarray(elevation, dtype=np.float64)
|
||||||
|
return (elevation, friction_mult, friction_raw, trails, barriers,
|
||||||
|
wilderness, mvum_by_mode, meta)
|
||||||
|
|
||||||
|
def _build_mvum_by_mode(self, bbox, shape):
|
||||||
|
"""Per-mode MVUM access rasters for the motorized modes, baked into the cost
|
||||||
|
layers (spec §9): get_mvum_access_grid -> 0=unknown/1=open/255=closed. Best-effort
|
||||||
|
-> returns None (graceful degradation, spec §16 risk register) when the MVUM DB is
|
||||||
|
unavailable (e.g. tests without navi.db)."""
|
||||||
|
mvum_mode = {"2w": "mtb", "4w": "atv", "vehicle": "vehicle"}
|
||||||
|
on_date = getattr(self, "mvum_on_date", None)
|
||||||
|
check = on_date.strftime("%m/%d") if on_date else None
|
||||||
|
out = {}
|
||||||
|
for mode, mv_name in mvum_mode.items():
|
||||||
|
try:
|
||||||
|
out[mode] = get_mvum_access_grid(
|
||||||
|
bbox["south"], bbox["north"], bbox["west"], bbox["east"],
|
||||||
|
target_shape=shape, mode=mv_name, check_date=check)
|
||||||
|
except Exception as e:
|
||||||
|
logger.info("auto: MVUM grid unavailable for %s: %s", mode, e)
|
||||||
|
return out or None
|
||||||
|
|
||||||
|
def _append_unified_segment(self, features, seg_coords, mode_idx):
|
||||||
|
"""One per-mode LineString feature (skipped if < 2 coords, e.g. a transition-only
|
||||||
|
cell). network_mode drives the map's per-segment colour (Phase 6)."""
|
||||||
|
if len(seg_coords) < 2:
|
||||||
|
return
|
||||||
|
features.append({
|
||||||
|
"type": "Feature",
|
||||||
|
"properties": {"segment_type": "unified", "mode": MODE_ORDER[mode_idx],
|
||||||
|
"network_mode": MODE_ORDER[mode_idx]},
|
||||||
|
"geometry": {"type": "LineString", "coordinates": list(seg_coords)},
|
||||||
|
})
|
||||||
|
|
||||||
|
def _render_unified_path(self, path, total_cost, meta, boundary_mode):
|
||||||
|
"""Render the (N,3) (row, col, mode) unified path into the GeoJSON response shape
|
||||||
|
(per-mode LineString segments + transition Point markers at mode-change cells + a
|
||||||
|
combined full-path line), matching _build_response / _build_hybrid_response.
|
||||||
|
selected_mode_set = sorted distinct modes used; NO auto_fallback_from (spec §7)."""
|
||||||
|
n = path.shape[0]
|
||||||
|
coords = []
|
||||||
|
for i in range(n):
|
||||||
|
lat, lon = self.dem_reader.pixel_to_latlon(int(path[i, 0]), int(path[i, 1]), meta)
|
||||||
|
coords.append([lon, lat])
|
||||||
|
modes = [int(path[i, 2]) for i in range(n)]
|
||||||
|
|
||||||
|
features = []
|
||||||
|
transitions = []
|
||||||
|
seg_start = 0
|
||||||
|
for i in range(1, n):
|
||||||
|
if modes[i] != modes[i - 1]:
|
||||||
|
self._append_unified_segment(features, coords[seg_start:i], modes[i - 1])
|
||||||
|
transitions.append((coords[i], modes[i - 1], modes[i]))
|
||||||
|
seg_start = i
|
||||||
|
self._append_unified_segment(features, coords[seg_start:n], modes[n - 1])
|
||||||
|
|
||||||
|
for (xy, fm, tm) in transitions:
|
||||||
|
features.append({
|
||||||
|
"type": "Feature",
|
||||||
|
"properties": {"segment_type": "transition", "kind": "transition",
|
||||||
|
"lat": xy[1], "lon": xy[0],
|
||||||
|
"from_mode": MODE_ORDER[fm], "to_mode": MODE_ORDER[tm]},
|
||||||
|
"geometry": {"type": "Point", "coordinates": [xy[0], xy[1]]},
|
||||||
|
})
|
||||||
|
|
||||||
|
combined = []
|
||||||
|
for xy in coords:
|
||||||
|
if not combined or combined[-1] != xy:
|
||||||
|
combined.append(xy)
|
||||||
|
if len(combined) >= 2:
|
||||||
|
features.append({
|
||||||
|
"type": "Feature",
|
||||||
|
"properties": {"segment_type": "combined", "boundary_mode": boundary_mode,
|
||||||
|
"scenario": "unified"},
|
||||||
|
"geometry": {"type": "LineString", "coordinates": combined},
|
||||||
|
})
|
||||||
|
|
||||||
|
total_m = sum(haversine_distance(combined[i][1], combined[i][0],
|
||||||
|
combined[i + 1][1], combined[i + 1][0])
|
||||||
|
for i in range(len(combined) - 1))
|
||||||
|
selected_mode_set = sorted({MODE_ORDER[m] for m in modes})
|
||||||
return {
|
return {
|
||||||
"status": "error",
|
"status": "ok",
|
||||||
"message": "No route found in any mode",
|
"route": {"type": "FeatureCollection", "features": features},
|
||||||
"selected_mode_set": mode_set,
|
"summary": {
|
||||||
|
"total_distance_km": total_m / 1000.0,
|
||||||
|
"total_effort_minutes": float(total_cost) / 60.0,
|
||||||
|
"scenario": "unified",
|
||||||
|
"boundary_mode": boundary_mode,
|
||||||
|
"selected_mode_set": selected_mode_set,
|
||||||
|
},
|
||||||
|
"selected_mode": selected_mode_set[0] if len(selected_mode_set) == 1 else "hybrid",
|
||||||
|
"selected_mode_set": selected_mode_set,
|
||||||
|
"scenario": "unified",
|
||||||
}
|
}
|
||||||
|
|
||||||
def _route_coords_latlon(self, result):
|
def _route_coords_latlon(self, result):
|
||||||
|
|
|
||||||
|
|
@ -312,6 +312,7 @@ def _typed_all(monkeypatch):
|
||||||
lambda self, cat: ALL_MODES)
|
lambda self, cat: ALL_MODES)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_picks_capability_mode(monkeypatch):
|
def test_route_auto_picks_capability_mode(monkeypatch):
|
||||||
# Classify-once: typed road endpoints -> intersection = all modes -> the first
|
# Classify-once: typed road endpoints -> intersection = all modes -> the first
|
||||||
# AUTO_MODE_PRIORITY mode (vehicle) is picked and routed ONCE (no 4-mode contest).
|
# AUTO_MODE_PRIORITY mode (vehicle) is picked and routed ONCE (no 4-mode contest).
|
||||||
|
|
@ -326,6 +327,7 @@ def test_route_auto_picks_capability_mode(monkeypatch):
|
||||||
assert calls == ["vehicle"] # ONE route call, not four
|
assert calls == ["vehicle"] # ONE route call, not four
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_falls_back_to_foot_on_error(monkeypatch):
|
def test_route_auto_falls_back_to_foot_on_error(monkeypatch):
|
||||||
# Foot-as-last-resort: the capability-picked mode (vehicle) fails, so Auto retries
|
# Foot-as-last-resort: the capability-picked mode (vehicle) fails, so Auto retries
|
||||||
# foot ONCE and ships it, tagging auto_fallback_from for the UI.
|
# foot ONCE and ships it, tagging auto_fallback_from for the UI.
|
||||||
|
|
@ -343,6 +345,7 @@ def test_route_auto_falls_back_to_foot_on_error(monkeypatch):
|
||||||
assert calls == ["vehicle", "foot"]
|
assert calls == ["vehicle", "foot"]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_returns_error_when_picked_and_foot_both_fail(monkeypatch):
|
def test_route_auto_returns_error_when_picked_and_foot_both_fail(monkeypatch):
|
||||||
# Picked mode AND the foot fallback both fail -> original error surfaces, exactly
|
# Picked mode AND the foot fallback both fail -> original error surfaces, exactly
|
||||||
# two attempts (picked, then foot).
|
# two attempts (picked, then foot).
|
||||||
|
|
@ -358,6 +361,7 @@ def test_route_auto_returns_error_when_picked_and_foot_both_fail(monkeypatch):
|
||||||
assert calls == ["vehicle", "foot"]
|
assert calls == ["vehicle", "foot"]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_no_fallback_when_picked_is_foot(monkeypatch):
|
def test_route_auto_no_fallback_when_picked_is_foot(monkeypatch):
|
||||||
# When the picked mode is already foot (foot-only intersection), there is no second
|
# When the picked mode is already foot (foot-only intersection), there is no second
|
||||||
# attempt -- foot cannot fall back to itself.
|
# attempt -- foot cannot fall back to itself.
|
||||||
|
|
@ -372,6 +376,7 @@ def test_route_auto_no_fallback_when_picked_is_foot(monkeypatch):
|
||||||
assert calls == ["foot"] # no fallback attempt
|
assert calls == ["foot"] # no fallback attempt
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_tagged_road_to_road_no_spatial_probe(monkeypatch):
|
def test_route_auto_tagged_road_to_road_no_spatial_probe(monkeypatch):
|
||||||
# Tagged road endpoints -> pure category classification, the spatial probe must
|
# Tagged road endpoints -> pure category classification, the spatial probe must
|
||||||
# NOT fire, and exactly one route call (mode=vehicle) is made.
|
# NOT fire, and exactly one route call (mode=vehicle) is made.
|
||||||
|
|
@ -388,6 +393,7 @@ def test_route_auto_tagged_road_to_road_no_spatial_probe(monkeypatch):
|
||||||
assert spatial_calls == [] # no spatial probe for tagged endpoints
|
assert spatial_calls == [] # no spatial probe for tagged endpoints
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_untagged_spatial_called_once_per_endpoint(monkeypatch):
|
def test_route_auto_untagged_spatial_called_once_per_endpoint(monkeypatch):
|
||||||
# One untagged endpoint -> _spatial_eligible_modes fires exactly once (for that
|
# One untagged endpoint -> _spatial_eligible_modes fires exactly once (for that
|
||||||
# endpoint only); the tagged endpoint stays a dict lookup.
|
# endpoint only); the tagged endpoint stays a dict lookup.
|
||||||
|
|
@ -405,6 +411,7 @@ def test_route_auto_untagged_spatial_called_once_per_endpoint(monkeypatch):
|
||||||
|
|
||||||
# ── _route_auto with category type hints (real _eligible_modes_from_category) ──
|
# ── _route_auto with category type hints (real _eligible_modes_from_category) ──
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_address_to_address_picks_vehicle(monkeypatch):
|
def test_route_auto_address_to_address_picks_vehicle(monkeypatch):
|
||||||
calls = []
|
calls = []
|
||||||
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
||||||
|
|
@ -415,6 +422,7 @@ def test_route_auto_address_to_address_picks_vehicle(monkeypatch):
|
||||||
assert calls == ["vehicle"]
|
assert calls == ["vehicle"]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_address_to_trailhead_picks_atv(monkeypatch):
|
def test_route_auto_address_to_trailhead_picks_atv(monkeypatch):
|
||||||
calls = []
|
calls = []
|
||||||
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
||||||
|
|
@ -426,6 +434,7 @@ def test_route_auto_address_to_trailhead_picks_atv(monkeypatch):
|
||||||
assert out["selected_mode_set"] == sorted({"4w", "2w", "foot"})
|
assert out["selected_mode_set"] == sorted({"4w", "2w", "foot"})
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_address_to_peak_picks_foot(monkeypatch):
|
def test_route_auto_address_to_peak_picks_foot(monkeypatch):
|
||||||
calls = []
|
calls = []
|
||||||
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
monkeypatch.setattr(OffrouteRouter, "route", _stub_route(_all_ok(), calls))
|
||||||
|
|
@ -437,6 +446,7 @@ def test_route_auto_address_to_peak_picks_foot(monkeypatch):
|
||||||
assert out["selected_mode_set"] == ["foot"]
|
assert out["selected_mode_set"] == ["foot"]
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_both_unknown_uses_spatial_fallback(monkeypatch):
|
def test_route_auto_both_unknown_uses_spatial_fallback(monkeypatch):
|
||||||
calls = []
|
calls = []
|
||||||
spatial_calls = []
|
spatial_calls = []
|
||||||
|
|
@ -861,6 +871,7 @@ def test_pathfind_wilderness_bbox_pad_is_1_5km(monkeypatch):
|
||||||
|
|
||||||
# ── Auto classify-once priority pick (replaces the old min-time contest) ──
|
# ── Auto classify-once priority pick (replaces the old min-time contest) ──
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_picks_priority_not_min_time(monkeypatch):
|
def test_route_auto_picks_priority_not_min_time(monkeypatch):
|
||||||
# Classify-once picks the first AUTO_MODE_PRIORITY mode in the intersection
|
# Classify-once picks the first AUTO_MODE_PRIORITY mode in the intersection
|
||||||
# (vehicle) and routes ONCE -- it no longer probes all modes to find a faster one.
|
# (vehicle) and routes ONCE -- it no longer probes all modes to find a faster one.
|
||||||
|
|
@ -882,6 +893,7 @@ def test_route_auto_picks_priority_not_min_time(monkeypatch):
|
||||||
assert calls == ["vehicle"] # routed once, no contest
|
assert calls == ["vehicle"] # routed once, no contest
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_per_leg_breakdown():
|
def test_route_auto_per_leg_breakdown():
|
||||||
# Scenario A (_build_response): foot wilderness leg + network leg -> both > 0.
|
# Scenario A (_build_response): foot wilderness leg + network leg -> both > 0.
|
||||||
r = object.__new__(OffrouteRouter)
|
r = object.__new__(OffrouteRouter)
|
||||||
|
|
@ -900,6 +912,7 @@ def test_route_auto_per_leg_breakdown():
|
||||||
assert abs((summ["wilderness_minutes"] + summ["network_minutes"]) - summ["total_effort_minutes"]) < 1e-6
|
assert abs((summ["wilderness_minutes"] + summ["network_minutes"]) - summ["total_effort_minutes"]) < 1e-6
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.skip(reason="superseded by unified-graph Phase 4")
|
||||||
def test_route_auto_annotates_picked_mode_once(monkeypatch):
|
def test_route_auto_annotates_picked_mode_once(monkeypatch):
|
||||||
# Classify-once routes the picked mode with annotate_mvum=False, then
|
# Classify-once routes the picked mode with annotate_mvum=False, then
|
||||||
# _annotate_network_segments runs exactly once, on that picked mode (vehicle).
|
# _annotate_network_segments runs exactly once, on that picked mode (vehicle).
|
||||||
|
|
@ -1302,3 +1315,222 @@ def test_compute_unified_cost_layers_perf():
|
||||||
elapsed = _time.perf_counter() - t0
|
elapsed = _time.perf_counter() - t0
|
||||||
assert set(layers["cost_mult"]) == {"foot", "2w", "4w", "vehicle"}
|
assert set(layers["cost_mult"]) == {"foot", "2w", "4w", "vehicle"}
|
||||||
assert elapsed <= 1.0, f"unified cost layers build took {elapsed:.3f}s > 1.0s"
|
assert elapsed <= 1.0, f"unified cost layers build took {elapsed:.3f}s > 1.0s"
|
||||||
|
|
||||||
|
|
||||||
|
# ── PHASE 4 — unified-graph _route_auto integration ───────────────────────────
|
||||||
|
# Hermetic: stub reader objects feed synthetic rasters, the REAL multimode kernel runs,
|
||||||
|
# transition DBs + MVUM + Valhalla are monkeypatched to deterministic empties.
|
||||||
|
import numpy as _p4np
|
||||||
|
import services.navi_offroute.router as _p4router
|
||||||
|
import services.navi_offroute.transitions as _p4trans
|
||||||
|
from services.navi_offroute.router import OffrouteRouter as _P4Router, MODE_ORDER as _MO
|
||||||
|
from services.navi_offroute.transitions import (_latlon_to_pixel as _ll2px,
|
||||||
|
_pixel_to_latlon as _px2ll)
|
||||||
|
from services.navi_offroute.astar import astar_multigoal_multimode as _mm4
|
||||||
|
from services.navi_offroute.cost import MODE_PROFILES as _MP4
|
||||||
|
|
||||||
|
|
||||||
|
def _p4_meta(rows, cols, cell_m=100.0):
|
||||||
|
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, "origin_lon": -111.0,
|
||||||
|
"cell_size_m": cell_m, "shape": (rows, cols)}
|
||||||
|
|
||||||
|
|
||||||
|
class _Grid:
|
||||||
|
def __init__(self, arr): self._arr = arr
|
||||||
|
def get_friction_grid(self, **kw): return self._arr
|
||||||
|
def get_barrier_grid(self, **kw): return self._arr
|
||||||
|
def get_trails_grid(self, **kw): return self._arr
|
||||||
|
def get_wilderness_grid(self, **kw): return self._arr
|
||||||
|
def close(self): pass
|
||||||
|
|
||||||
|
|
||||||
|
class _StubDem:
|
||||||
|
def __init__(self, elevation, meta): self._e, self._m = elevation, meta
|
||||||
|
def get_elevation_grid(self, **kw): return self._e, self._m
|
||||||
|
def latlon_to_pixel(self, lat, lon, meta): return _ll2px(lat, lon, meta)
|
||||||
|
def pixel_to_latlon(self, row, col, meta): return _px2ll(row, col, meta)
|
||||||
|
def close(self): pass
|
||||||
|
|
||||||
|
|
||||||
|
class _StubIdx:
|
||||||
|
def __init__(self, recs): self._r = recs or []
|
||||||
|
def query_parking_near_line(self, coords, buffer_m=2000): return self._r
|
||||||
|
def query_trailheads_near_line(self, coords, buffer_m=2000): return self._r
|
||||||
|
|
||||||
|
|
||||||
|
def _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, eligible):
|
||||||
|
"""An OffrouteRouter wired with synthetic rasters + deterministic-empty DBs/MVUM.
|
||||||
|
`eligible(lat, lon) -> frozenset` supplies the per-endpoint seed modes (spatial probe)."""
|
||||||
|
r = _P4Router()
|
||||||
|
r.dem_reader = _StubDem(elevation.astype(_p4np.float64), meta)
|
||||||
|
r.friction_reader = _Grid(friction_raw)
|
||||||
|
r.barrier_reader = _Grid(barriers)
|
||||||
|
r.trail_reader = _Grid(trails)
|
||||||
|
r.wilderness_reader = _Grid(_p4np.zeros_like(barriers))
|
||||||
|
monkeypatch.setattr(_p4router, "get_mvum_access_grid",
|
||||||
|
lambda *a, **k: (_ for _ in ()).throw(RuntimeError("no mvum db")))
|
||||||
|
monkeypatch.setattr(_p4trans, "load_parking_index", lambda *a, **k: _StubIdx([]))
|
||||||
|
monkeypatch.setattr(_p4trans, "load_trailheads", lambda *a, **k: _StubIdx([]))
|
||||||
|
monkeypatch.setattr(_p4trans, "get_surface_change_candidates", lambda *a, **k: [])
|
||||||
|
monkeypatch.setattr(_P4Router, "_spatial_eligible_modes",
|
||||||
|
lambda self, lat, lon, cache: eligible(lat, lon))
|
||||||
|
return r
|
||||||
|
|
||||||
|
|
||||||
|
def _unified_segments(result):
|
||||||
|
return [f for f in result["route"]["features"]
|
||||||
|
if (f["properties"] or {}).get("segment_type") == "unified"]
|
||||||
|
|
||||||
|
|
||||||
|
def _transition_feats(result):
|
||||||
|
return [f for f in result["route"]["features"]
|
||||||
|
if (f["properties"] or {}).get("segment_type") == "transition"]
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_wilderness_to_home_walk_then_drive(monkeypatch):
|
||||||
|
# §1 failure case: wilderness start (foot-only) -> long road to an addressed end.
|
||||||
|
rows, cols = 3, 40
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
friction_raw = _p4np.full((rows, cols), 10, dtype=_p4np.uint8) # forest: foot ok, vehicle inf
|
||||||
|
trails = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
trails[1, 8:40] = 5 # road from col 8 -> terminus at 8
|
||||||
|
barriers = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
meta = _p4_meta(rows, cols)
|
||||||
|
s_lat, s_lon = _px2ll(1, 0, meta) # wilderness start
|
||||||
|
e_lat, e_lon = _px2ll(1, 39, meta) # on-road end
|
||||||
|
elig = lambda lat, lon: (frozenset({"foot"}) if abs(lon - s_lon) < 1e-9
|
||||||
|
else frozenset({"foot", "2w", "4w", "vehicle"}))
|
||||||
|
r = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, elig)
|
||||||
|
|
||||||
|
out = r._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||||
|
assert out["status"] == "ok", out
|
||||||
|
assert "foot" in out["selected_mode_set"] and "vehicle" in out["selected_mode_set"]
|
||||||
|
segs = _unified_segments(out)
|
||||||
|
assert segs[0]["properties"]["network_mode"] == "foot"
|
||||||
|
assert segs[-1]["properties"]["network_mode"] == "vehicle"
|
||||||
|
trans = _transition_feats(out)
|
||||||
|
assert len(trans) == 1
|
||||||
|
assert _ll2px(trans[0]["properties"]["lat"], trans[0]["properties"]["lon"], meta) == (1, 8)
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_foot_to_offpath(monkeypatch):
|
||||||
|
rows, cols = 3, 12
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
friction_raw = _p4np.full((rows, cols), 30, dtype=_p4np.uint8) # grass, foot passable
|
||||||
|
trails = _p4np.zeros((rows, cols), dtype=_p4np.uint8) # no network at all
|
||||||
|
barriers = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
meta = _p4_meta(rows, cols)
|
||||||
|
s_lat, s_lon = _px2ll(1, 0, meta)
|
||||||
|
e_lat, e_lon = _px2ll(1, 11, meta)
|
||||||
|
elig = lambda lat, lon: frozenset({"foot"})
|
||||||
|
r = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, elig)
|
||||||
|
|
||||||
|
out = r._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||||
|
assert out["status"] == "ok", out
|
||||||
|
assert out["selected_mode_set"] == ["foot"]
|
||||||
|
assert _transition_feats(out) == []
|
||||||
|
assert all(s["properties"]["network_mode"] == "foot" for s in _unified_segments(out))
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_road_to_road(monkeypatch):
|
||||||
|
rows, cols = 3, 20
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
friction_raw = _p4np.full((rows, cols), 10, dtype=_p4np.uint8) # off-road forest (vehicle inf)
|
||||||
|
trails = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
trails[1, :] = 5 # road spans the grid
|
||||||
|
barriers = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
meta = _p4_meta(rows, cols)
|
||||||
|
s_lat, s_lon = _px2ll(1, 0, meta)
|
||||||
|
e_lat, e_lon = _px2ll(1, 19, meta)
|
||||||
|
elig = lambda lat, lon: frozenset({"foot", "2w", "4w", "vehicle"})
|
||||||
|
r = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, elig)
|
||||||
|
|
||||||
|
out = r._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||||
|
assert out["status"] == "ok", out
|
||||||
|
assert "vehicle" in out["selected_mode_set"]
|
||||||
|
# No off-network excursion: every combined-path cell sits on the road (trail != 0).
|
||||||
|
combined = [f for f in out["route"]["features"]
|
||||||
|
if f["properties"].get("segment_type") == "combined"][0]
|
||||||
|
for lon, lat in combined["geometry"]["coordinates"]:
|
||||||
|
rr, cc = _ll2px(lat, lon, meta)
|
||||||
|
assert trails[rr, cc] != 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_heuristic_admissibility_road_case():
|
||||||
|
# §10 fix: with a fast road (friction 0.1) the A* heuristic must stay admissible, so
|
||||||
|
# the heuristic-guided cost equals the disable_heuristic=True (Dijkstra) cost.
|
||||||
|
rows, cols, nm = 3, 20, 4
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
trail = _p4np.zeros((rows, cols), dtype=_p4np.uint8); trail[1, :] = 5
|
||||||
|
stack = _p4np.full((rows, cols, nm), _p4np.inf)
|
||||||
|
stack[:, :, 0] = 1.0 # foot passable off-trail
|
||||||
|
tfs = _p4np.full((nm, 256), _p4np.inf)
|
||||||
|
tfs[0, 5] = 0.1; tfs[3, 5] = 0.1 # foot + vehicle on road
|
||||||
|
mg = _p4np.array([_p4np.tan(_p4np.radians(_MP4[m].max_slope_deg)) for m in _MO])
|
||||||
|
sfid = _p4np.array([{"tobler": 0, "herzog": 1, "linear": 2}[_MP4[m].speed_function]
|
||||||
|
for m in _MO], dtype=_p4np.int64)
|
||||||
|
base = _p4np.array([_MP4[m].base_speed_kmh for m in _MO])
|
||||||
|
barr = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
empty_i = _p4np.empty(0, dtype=_p4np.int64); empty_f = _p4np.empty(0, dtype=_p4np.float64)
|
||||||
|
seed = _p4np.array([3], dtype=_p4np.int64) # vehicle origin + goal
|
||||||
|
gr = _p4np.array([1], dtype=_p4np.int64); gc = _p4np.array([19], dtype=_p4np.int64)
|
||||||
|
args = (stack, elevation, 100.0, 100.0, mg, sfid, base, trail, tfs, barr, 1,
|
||||||
|
1, 0, seed, gr, gc, seed, empty_i, empty_i, empty_i, empty_i, empty_f)
|
||||||
|
_, _, cost_h = _mm4(*args)
|
||||||
|
_, _, cost_dijkstra = _mm4(*args, disable_heuristic=True)
|
||||||
|
assert _p4np.isfinite(cost_h) and _p4np.isfinite(cost_dijkstra)
|
||||||
|
assert cost_h <= cost_dijkstra + 1e-6
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_no_auto_fallback_from_field(monkeypatch):
|
||||||
|
rows, cols = 3, 10
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
friction_raw = _p4np.full((rows, cols), 30, dtype=_p4np.uint8)
|
||||||
|
trails = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
barriers = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
meta = _p4_meta(rows, cols)
|
||||||
|
s_lat, s_lon = _px2ll(1, 0, meta); e_lat, e_lon = _px2ll(1, 9, meta)
|
||||||
|
r = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta,
|
||||||
|
lambda lat, lon: frozenset({"foot"}))
|
||||||
|
out = r._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||||
|
assert out["status"] == "ok"
|
||||||
|
assert "auto_fallback_from" not in out
|
||||||
|
assert "auto_fallback_from" not in (out.get("summary") or {})
|
||||||
|
|
||||||
|
|
||||||
|
def _on_network_count(result, trails, meta):
|
||||||
|
combined = [f for f in result["route"]["features"]
|
||||||
|
if f["properties"].get("segment_type") == "combined"][0]
|
||||||
|
n = 0
|
||||||
|
for lon, lat in combined["geometry"]["coordinates"]:
|
||||||
|
rr, cc = _ll2px(lat, lon, meta)
|
||||||
|
if trails[rr, cc] != 0:
|
||||||
|
n += 1
|
||||||
|
return n
|
||||||
|
|
||||||
|
|
||||||
|
def test_route_auto_network_affinity_biases_path(monkeypatch):
|
||||||
|
# Road (row 0, fast) vs flat grass field (drivable, slower). network_affinity > 1
|
||||||
|
# penalises on-network edges; pushing it high across the modes biases the path off the
|
||||||
|
# network. (foot is always an eligible seed, so a single-mode affinity is escaped by a
|
||||||
|
# mode switch -- the bias must cover the modes that can ride the road.)
|
||||||
|
rows, cols = 4, 14
|
||||||
|
elevation = _p4np.full((rows, cols), 1000.0)
|
||||||
|
friction_raw = _p4np.full((rows, cols), 30, dtype=_p4np.uint8) # flat grass: off-road drivable
|
||||||
|
trails = _p4np.zeros((rows, cols), dtype=_p4np.uint8); trails[0, :] = 5
|
||||||
|
barriers = _p4np.zeros((rows, cols), dtype=_p4np.uint8)
|
||||||
|
meta = _p4_meta(rows, cols)
|
||||||
|
s_lat, s_lon = _px2ll(0, 0, meta); e_lat, e_lon = _px2ll(0, 13, meta)
|
||||||
|
elig = lambda lat, lon: frozenset({"foot", "2w", "4w", "vehicle"})
|
||||||
|
affinity = {m: 80.0 for m in ("foot", "2w", "4w", "vehicle")}
|
||||||
|
|
||||||
|
r1 = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, elig)
|
||||||
|
base = r1._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||||
|
r2 = _auto_router(monkeypatch, elevation, friction_raw, trails, barriers, meta, elig)
|
||||||
|
biased = r2._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic", network_affinity=affinity)
|
||||||
|
assert base["status"] == "ok" and biased["status"] == "ok"
|
||||||
|
assert _on_network_count(biased, trails, meta) < _on_network_count(base, trails, meta)
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue