mirror of
https://github.com/zvx-echo6/navi.git
synced 2026-08-26 17:31:37 +00:00
navi-offroute: HPA* runtime kernel + router dispatch (Phase H3)
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
mj
parent
86cd15f817
commit
6a60600021
3 changed files with 480 additions and 1 deletions
|
|
@ -12,7 +12,12 @@ Cliffs (|grade| > max_grade) incur a smooth exponential penalty rather than a ha
|
|||
wall, so a single noisy DEM cell can't fabricate an impassable edge; only truly
|
||||
absurd grades (penalty > SLOPE_PENALTY_CAP) are dropped.
|
||||
"""
|
||||
import hashlib
|
||||
import heapq
|
||||
import itertools
|
||||
import math
|
||||
import sqlite3
|
||||
from collections import defaultdict
|
||||
|
||||
import numpy as np
|
||||
from numba import njit
|
||||
|
|
@ -597,3 +602,231 @@ def astar_multigoal_multimode(
|
|||
break
|
||||
|
||||
return -1, np.empty((0, 3), dtype=np.int64), INF
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════════════════
|
||||
# HPA* TWO-LEVEL RUNTIME (unified-graph perf, HPA-SPEC.md §8/§9, Phase H3)
|
||||
# ═══════════════════════════════════════════════════════════════════════════════
|
||||
#
|
||||
# astar_hpa_multimode is a PURE-PYTHON sibling of astar_multigoal_multimode (NOT @njit:
|
||||
# it does SQLite I/O + per-chunk Python loops). It searches the precomputed abstract chunk
|
||||
# graph (cost tiles from hpa_build), then refines each hop with the existing @njit
|
||||
# astar_multigoal on that chunk's live cost layer. astar_multigoal / astar_multigoal_multimode
|
||||
# are byte-unchanged; HPA* engages only when the dispatcher passes a tile DB.
|
||||
#
|
||||
# v1 limitations (HPA-SPEC.md §5/§8, PR #44): border entrances ONLY (no transition-cell
|
||||
# entrances ≥20), so there are NO mode-switch edges -> the abstract path is SINGLE-MODE
|
||||
# (a mode m in start_modes ∩ goal_modes). A route whose optimum needs a mode switch (the §1
|
||||
# wilderness→home walk-then-drive) will here degrade to single-mode or find no path; the
|
||||
# dispatcher then falls back to astar_multigoal_multimode. **This means enabling HPA* on a
|
||||
# mixed-mode route can return a worse selected_mode_set than unified A* — keep disabled in
|
||||
# prod until transition-cell entrances land (a follow-up) or H5 gates it.**
|
||||
# The abstract search uses Dijkstra (h≡0, trivially admissible/optimal); the abstract graph
|
||||
# is tiny, so a §10-style heuristic isn't needed for v1.
|
||||
|
||||
HPA_BORDER_ENTRANCES = 20
|
||||
|
||||
|
||||
def _hpa_profile_hash():
|
||||
from .cost import MODE_PROFILES
|
||||
return hashlib.sha256(repr(MODE_PROFILES).encode()).hexdigest()
|
||||
|
||||
|
||||
def _hpa_coverage_reason(conn, needed_chunks, boundary_mode, network_affinity):
|
||||
"""Return a fallback reason string (HPA cannot/should-not engage), or None if clear.
|
||||
Order: config gates first (cheap), then freshness, then tile coverage."""
|
||||
if boundary_mode != "pragmatic":
|
||||
return "boundary_mode"
|
||||
if network_affinity and any(float(v) != 1.0 for v in network_affinity.values()):
|
||||
return "affinity"
|
||||
row = conn.execute("SELECT value FROM meta WHERE key='mode_profile_hash'").fetchone()
|
||||
if not row or row[0] != _hpa_profile_hash():
|
||||
return "stale_profile"
|
||||
cxs = [c[0] for c in needed_chunks]
|
||||
cys = [c[1] for c in needed_chunks]
|
||||
present = {(r[0], r[1]) for r in conn.execute(
|
||||
"SELECT DISTINCT chunk_x, chunk_y FROM chunk_costs "
|
||||
"WHERE chunk_x BETWEEN ? AND ? AND chunk_y BETWEEN ? AND ?",
|
||||
(min(cxs), max(cxs), min(cys), max(cys)))}
|
||||
if any(ch not in present for ch in needed_chunks):
|
||||
return "missing_chunk"
|
||||
return None
|
||||
|
||||
|
||||
def _hpa_abstract_search(conn, needed_chunks, relevant_modes, start_edges, goal_edges):
|
||||
"""Dijkstra over the abstract graph. Nodes are (cx, cy, entrance, mode) plus virtual
|
||||
"START"/"GOAL". Edges: intra-chunk (precomputed tile costs), inter-chunk seams (free,
|
||||
same physical border cell), and the start/goal pseudo-edges. Returns
|
||||
(node_seq_excl_endpoints, total_cost) or (None, INF). v1: no cross-mode edges."""
|
||||
present = set(needed_chunks)
|
||||
rmodes = set(relevant_modes)
|
||||
cxs = [c[0] for c in needed_chunks]
|
||||
cys = [c[1] for c in needed_chunks]
|
||||
adj = defaultdict(list)
|
||||
# Intra-chunk directed edges (border entrances 0..19 only — transition cells deferred).
|
||||
for cx, cy, m, fe, te, cost in conn.execute(
|
||||
"SELECT chunk_x, chunk_y, mode_idx, from_entrance, to_entrance, cost_s FROM chunk_costs "
|
||||
"WHERE chunk_x BETWEEN ? AND ? AND chunk_y BETWEEN ? AND ?",
|
||||
(min(cxs), max(cxs), min(cys), max(cys))):
|
||||
if m in rmodes and fe < HPA_BORDER_ENTRANCES and te < HPA_BORDER_ENTRANCES:
|
||||
adj[(cx, cy, fe, m)].append(((cx, cy, te, m), float(cost)))
|
||||
# Inter-chunk seams (free, both directions). Right 5..9 ↔ left 15..19 of (cx+1,cy);
|
||||
# bottom 10..14 ↔ top 0..4 of (cx,cy+1) — same fraction, same physical cell (spec §8).
|
||||
for (cx, cy) in needed_chunks:
|
||||
for m in rmodes:
|
||||
if (cx + 1, cy) in present:
|
||||
for k in range(5):
|
||||
a, b = (cx, cy, 5 + k, m), (cx + 1, cy, 15 + k, m)
|
||||
adj[a].append((b, 0.0)); adj[b].append((a, 0.0))
|
||||
if (cx, cy + 1) in present:
|
||||
for k in range(5):
|
||||
a, b = (cx, cy, 10 + k, m), (cx, cy + 1, k, m)
|
||||
adj[a].append((b, 0.0)); adj[b].append((a, 0.0))
|
||||
for node, cost in start_edges.items():
|
||||
adj["START"].append((node, float(cost)))
|
||||
for node, cost in goal_edges.items():
|
||||
adj[node].append(("GOAL", float(cost)))
|
||||
|
||||
counter = itertools.count()
|
||||
dist = {"START": 0.0}
|
||||
prev = {}
|
||||
pq = [(0.0, next(counter), "START")]
|
||||
while pq:
|
||||
d, _, u = heapq.heappop(pq)
|
||||
if u == "GOAL":
|
||||
break
|
||||
if d > dist.get(u, INF):
|
||||
continue
|
||||
for v, w in adj[u]:
|
||||
nd = d + w
|
||||
if nd < dist.get(v, INF):
|
||||
dist[v] = nd
|
||||
prev[v] = u
|
||||
heapq.heappush(pq, (nd, next(counter), v))
|
||||
if "GOAL" not in dist:
|
||||
return None, INF
|
||||
seq, node = [], "GOAL"
|
||||
while node != "START":
|
||||
if node != "GOAL":
|
||||
seq.append(node)
|
||||
node = prev[node]
|
||||
seq.reverse()
|
||||
return seq, dist["GOAL"]
|
||||
|
||||
|
||||
def _hpa_inchunk(layer, fr, fc, gr, gc):
|
||||
"""Least-time path + cost between two cells of a chunk's live cost layer (single mode)."""
|
||||
_, path, cost = astar_multigoal(
|
||||
layer["cost_mult"], layer["elevation"], layer["cell_size_m"], layer["cell_size_m"],
|
||||
layer["max_grade"], layer["speed_function_id"], layer["base_speed_kmh"],
|
||||
layer["trail_grid"], layer["trail_friction_lookup"], layer["barrier_grid"], 1,
|
||||
int(fr), int(fc), np.array([gr], dtype=np.int64), np.array([gc], dtype=np.int64))
|
||||
return path, cost
|
||||
|
||||
|
||||
def _hpa_emit(layer, path, mode, dem_reader, full_meta, out):
|
||||
"""Append a chunk-local cell path to `out` as (full_row, full_col, mode), via lat/lon
|
||||
(chunk grid -> full-bbox grid) since the chunk fetch and full fetch have different pixel
|
||||
origins. Consecutive duplicates (e.g. at seams) are dropped."""
|
||||
for k in range(path.shape[0]):
|
||||
lat, lon = dem_reader.pixel_to_latlon(int(path[k, 0]), int(path[k, 1]), layer["meta"])
|
||||
r, c = dem_reader.latlon_to_pixel(lat, lon, full_meta)
|
||||
node = (int(r), int(c), int(mode))
|
||||
if not out or out[-1] != node:
|
||||
out.append(node)
|
||||
|
||||
|
||||
def astar_hpa_multimode(tile_db_path, full_meta, start_lat, start_lon, end_lat, end_lon,
|
||||
origin_modes, goal_modes, boundary_mode, network_affinity,
|
||||
chunk_layer=None, dem_reader=None):
|
||||
"""Two-level HPA* (HPA-SPEC.md §8). Returns (idx, path_Nx3_int64, total_cost, reason)
|
||||
matching astar_multigoal_multimode's render contract: idx==0 on success (reason None),
|
||||
idx==-1 on fallback (reason in {boundary_mode, affinity, stale_profile, missing_chunk,
|
||||
no_abstract_path, refine_failed}). chunk_layer(cx, cy, mode_idx)->layer dict (live,
|
||||
native-30m, tile-grid-aligned) is supplied by the dispatcher for refinement."""
|
||||
from . import hpa_build as hb
|
||||
empty = np.empty((0, 3), dtype=np.int64)
|
||||
south, north, west, east = full_meta["bounds"]
|
||||
needed = hb.chunks_in_bbox(south, west, north, east)
|
||||
|
||||
conn = sqlite3.connect(f"file:{tile_db_path}?mode=ro", uri=True)
|
||||
try:
|
||||
reason = _hpa_coverage_reason(conn, needed, boundary_mode, network_affinity)
|
||||
if reason is not None:
|
||||
return -1, empty, INF, reason
|
||||
relevant = sorted(set(int(x) for x in origin_modes) & set(int(x) for x in goal_modes))
|
||||
if not relevant:
|
||||
return -1, empty, INF, "no_abstract_path"
|
||||
|
||||
start_chunk = hb.chunk_coords(start_lat, start_lon)
|
||||
goal_chunk = hb.chunk_coords(end_lat, end_lon)
|
||||
|
||||
# Same chunk: a direct in-chunk A* beats routing out to a border entrance and back.
|
||||
if start_chunk == goal_chunk:
|
||||
best = None
|
||||
for m in relevant:
|
||||
L = chunk_layer(start_chunk[0], start_chunk[1], m)
|
||||
sr, sc = dem_reader.latlon_to_pixel(start_lat, start_lon, L["meta"])
|
||||
gr, gc = dem_reader.latlon_to_pixel(end_lat, end_lon, L["meta"])
|
||||
path, cost = _hpa_inchunk(L, sr, sc, gr, gc)
|
||||
if np.isfinite(cost) and (best is None or cost < best[0]):
|
||||
best = (cost, L, path, m)
|
||||
if best is None:
|
||||
return -1, empty, INF, "no_abstract_path"
|
||||
out = []
|
||||
_hpa_emit(best[1], best[2], best[3], dem_reader, full_meta, out)
|
||||
return (0, np.array(out, dtype=np.int64), best[0], None) if len(out) >= 2 \
|
||||
else (-1, empty, INF, "refine_failed")
|
||||
|
||||
# Pseudo-edges: start cell -> each start-chunk entrance; each goal-chunk entrance -> end.
|
||||
start_edges, goal_edges = {}, {}
|
||||
for m in relevant:
|
||||
Ls = chunk_layer(start_chunk[0], start_chunk[1], m)
|
||||
sr, sc = dem_reader.latlon_to_pixel(start_lat, start_lon, Ls["meta"])
|
||||
for ei, (er, ec) in enumerate(Ls["entrance_cells"]):
|
||||
_, cost = _hpa_inchunk(Ls, sr, sc, er, ec)
|
||||
if np.isfinite(cost):
|
||||
start_edges[(start_chunk[0], start_chunk[1], ei, m)] = cost
|
||||
Lg = chunk_layer(goal_chunk[0], goal_chunk[1], m)
|
||||
gr, gc = dem_reader.latlon_to_pixel(end_lat, end_lon, Lg["meta"])
|
||||
for ei, (er, ec) in enumerate(Lg["entrance_cells"]):
|
||||
_, cost = _hpa_inchunk(Lg, er, ec, gr, gc)
|
||||
if np.isfinite(cost):
|
||||
goal_edges[(goal_chunk[0], goal_chunk[1], ei, m)] = cost
|
||||
|
||||
seq, cost = _hpa_abstract_search(conn, needed, relevant, start_edges, goal_edges)
|
||||
if seq is None:
|
||||
return -1, empty, INF, "no_abstract_path"
|
||||
finally:
|
||||
conn.close()
|
||||
|
||||
# Refinement: stitch the real cells for each hop. Single mode throughout (v1).
|
||||
m = seq[0][3]
|
||||
out = []
|
||||
Ls = chunk_layer(start_chunk[0], start_chunk[1], m)
|
||||
sr, sc = dem_reader.latlon_to_pixel(start_lat, start_lon, Ls["meta"])
|
||||
fr, fc = Ls["entrance_cells"][seq[0][2]]
|
||||
path, c = _hpa_inchunk(Ls, sr, sc, fr, fc)
|
||||
if not np.isfinite(c):
|
||||
return -1, np.empty((0, 3), dtype=np.int64), INF, "refine_failed"
|
||||
_hpa_emit(Ls, path, m, dem_reader, full_meta, out)
|
||||
for a, b in zip(seq, seq[1:]):
|
||||
if a[0] == b[0] and a[1] == b[1]: # intra-chunk hop -> refine
|
||||
L = chunk_layer(a[0], a[1], a[3])
|
||||
ar, ac = L["entrance_cells"][a[2]]
|
||||
br, bc = L["entrance_cells"][b[2]]
|
||||
path, c = _hpa_inchunk(L, ar, ac, br, bc)
|
||||
if not np.isfinite(c):
|
||||
return -1, np.empty((0, 3), dtype=np.int64), INF, "refine_failed"
|
||||
_hpa_emit(L, path, m, dem_reader, full_meta, out)
|
||||
# else: inter-chunk seam (same physical cell) -> no refinement
|
||||
Lg = chunk_layer(goal_chunk[0], goal_chunk[1], m)
|
||||
lr, lc = Lg["entrance_cells"][seq[-1][2]]
|
||||
gr, gc = dem_reader.latlon_to_pixel(end_lat, end_lon, Lg["meta"])
|
||||
path, c = _hpa_inchunk(Lg, lr, lc, gr, gc)
|
||||
if not np.isfinite(c):
|
||||
return -1, np.empty((0, 3), dtype=np.int64), INF, "refine_failed"
|
||||
_hpa_emit(Lg, path, m, dem_reader, full_meta, out)
|
||||
if len(out) < 2:
|
||||
return -1, np.empty((0, 3), dtype=np.int64), INF, "refine_failed"
|
||||
return 0, np.array(out, dtype=np.int64), cost, None
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@ import requests
|
|||
import psycopg2
|
||||
import psycopg2.extras
|
||||
from shapely.geometry import LineString, Point
|
||||
from .astar import astar_multigoal, astar_multigoal_multimode, inflate_cost_multiplier
|
||||
from .astar import (astar_multigoal, astar_multigoal_multimode, astar_hpa_multimode,
|
||||
inflate_cost_multiplier)
|
||||
from .mvum_surface_change import get_surface_change_candidates
|
||||
from .mvum_parking import load_parking_index # noqa: F401 (singleton injected by handler)
|
||||
|
||||
|
|
@ -60,6 +61,11 @@ POSTGIS_DSN = os.environ.get("NAVI_OFFROUTE_POSTGIS_DSN", "dbname=padus")
|
|||
# Valhalla endpoint (recon-side network router, HTTP)
|
||||
VALHALLA_URL = os.environ.get("NAVI_OFFROUTE_VALHALLA_URL", "http://localhost:8002")
|
||||
|
||||
# HPA* cost-tile DB (HPA-SPEC.md §8/§9, Phase H3). Unset (None) -> HPA* never engages and
|
||||
# Auto routing is byte-identical to the unified-graph path. Set to a tile DB (built by
|
||||
# hpa_build) to enable the two-level fast path for covered, pragmatic, no-affinity routes.
|
||||
HPA_TILE_DB = os.environ.get("NAVI_OFFROUTE_HPA_DB")
|
||||
|
||||
# Search radius for entry points (km)
|
||||
DEFAULT_SEARCH_RADIUS_KM = 50
|
||||
EXPANDED_SEARCH_RADIUS_KM = 100
|
||||
|
|
@ -887,6 +893,27 @@ class OffrouteRouter:
|
|||
origin_modes = np.array(sorted(MODE_INDEX[m] for m in start_eligible), dtype=np.int64)
|
||||
goal_modes = np.array(sorted(MODE_INDEX[m] for m in end_eligible), dtype=np.int64)
|
||||
|
||||
# HPA* fast path (Phase H3): when a tile DB is configured + covers the route, search
|
||||
# the precomputed abstract chunk graph instead of flooding the full bbox. Whole-route
|
||||
# fallback to the unified kernel below on any miss (HPA-SPEC.md §8/§9). When
|
||||
# NAVI_OFFROUTE_HPA_DB is unset this block is skipped entirely (behaviour unchanged).
|
||||
if self._hpa_eligible(boundary_mode, network_affinity):
|
||||
_h0 = time.perf_counter()
|
||||
_cache = {"raster": {}, "layer": {}}
|
||||
hidx, hpath, hcost, hreason = astar_hpa_multimode(
|
||||
HPA_TILE_DB, meta, start_lat, start_lon, end_lat, end_lon,
|
||||
origin_modes, goal_modes, boundary_mode, network_affinity,
|
||||
chunk_layer=lambda cx, cy, mi: self._hpa_chunk_layer(cx, cy, mi, _cache),
|
||||
dem_reader=self.dem_reader)
|
||||
if hidx >= 0 and hpath.shape[0] > 0:
|
||||
logger.info("auto: HPA* (chunks=%d) in %.2fs",
|
||||
len(_cache["raster"]), time.perf_counter() - _h0)
|
||||
return self._render_unified_path(hpath, hcost, meta, boundary_mode)
|
||||
logger.info("auto: HPA fallback reason=%s -> unified A*", hreason)
|
||||
elif HPA_TILE_DB and os.path.exists(HPA_TILE_DB):
|
||||
_r = "boundary_mode" if boundary_mode != "pragmatic" else "affinity"
|
||||
logger.info("auto: HPA fallback reason=%s -> unified A*", _r)
|
||||
|
||||
# 7. Unpack transition cells into the kernel's flat 1D arrays.
|
||||
tc = layers["transition_cells"]
|
||||
nt = len(tc)
|
||||
|
|
@ -1059,6 +1086,68 @@ class OffrouteRouter:
|
|||
"scenario": "unified",
|
||||
}
|
||||
|
||||
def _hpa_eligible(self, boundary_mode, network_affinity):
|
||||
"""HPA* engages only with a configured + existing tile DB, the default boundary mode,
|
||||
and no network_affinity — the tiles are pure-terrain/pragmatic (HPA-SPEC.md §8), so
|
||||
other configs would change the answer and must use the unified fallback."""
|
||||
if not (HPA_TILE_DB and os.path.exists(HPA_TILE_DB)):
|
||||
return False
|
||||
if boundary_mode != "pragmatic":
|
||||
return False
|
||||
if network_affinity and any(float(v) != 1.0 for v in network_affinity.values()):
|
||||
return False
|
||||
return True
|
||||
|
||||
def _hpa_chunk_layer(self, cx, cy, mode_idx, cache):
|
||||
"""Live native-30m cost layer for one chunk (tile-grid-aligned), for HPA* refinement.
|
||||
Pure terrain only (no MVUM/barriers/network_affinity/corridor-mask), matching the H2
|
||||
tile build so entrance cells and costs line up. Rasters cached per chunk, layers per
|
||||
(chunk, mode)."""
|
||||
from . import hpa_build as hb
|
||||
if (cx, cy) not in cache["raster"]:
|
||||
s, w, n, e = hb.chunk_bounds(cx, cy)
|
||||
elev, cmeta = self.dem_reader.get_elevation_grid(south=s, north=n, west=w, east=e)
|
||||
shape = elev.shape
|
||||
fraw = self.friction_reader.get_friction_grid(
|
||||
south=s, north=n, west=w, east=e, target_shape=shape)
|
||||
fmult = friction_to_multiplier(fraw)
|
||||
trails = self.trail_reader.get_trails_grid(
|
||||
south=s, north=n, west=w, east=e, target_shape=shape)
|
||||
wild = None
|
||||
if self.wilderness_reader is not None:
|
||||
wild = self.wilderness_reader.get_wilderness_grid(
|
||||
south=s, north=n, west=w, east=e, target_shape=shape)
|
||||
cache["raster"][(cx, cy)] = (
|
||||
np.ascontiguousarray(elev, np.float64), fmult, fraw, trails, wild, cmeta)
|
||||
elev, fmult, fraw, trails, wild, cmeta = cache["raster"][(cx, cy)]
|
||||
key = (cx, cy, mode_idx)
|
||||
if key not in cache["layer"]:
|
||||
mode = MODE_ORDER[mode_idx]
|
||||
prof = MODE_PROFILES[mode]
|
||||
cs = float(cmeta["cell_size_m"])
|
||||
cm = compute_cost_multiplier_grid(
|
||||
elev, cell_size_lat_m=cs, cell_size_lon_m=cs,
|
||||
friction=fmult, friction_raw=fraw, wilderness=wild, mode=mode)
|
||||
cm = np.ascontiguousarray(inflate_cost_multiplier(cm), np.float64)
|
||||
tfl = np.full(256, np.inf, np.float64)
|
||||
for tv, fr in prof.trail_friction.items():
|
||||
tfl[tv] = np.inf if fr is None else float(fr)
|
||||
shape = elev.shape
|
||||
cache["layer"][key] = {
|
||||
"cost_mult": cm, "elevation": elev,
|
||||
"trail_grid": np.ascontiguousarray(
|
||||
trails if trails is not None else np.zeros(shape, np.uint8), np.uint8),
|
||||
"trail_friction_lookup": tfl,
|
||||
"barrier_grid": np.zeros(shape, np.uint8),
|
||||
"max_grade": float(np.tan(np.radians(prof.max_slope_deg))),
|
||||
"speed_function_id": {"tobler": 0, "herzog": 1, "linear": 2}.get(prof.speed_function, 0),
|
||||
"base_speed_kmh": float(prof.base_speed_kmh),
|
||||
"cell_size_m": cs,
|
||||
"entrance_cells": hb._entrance_cells(*shape),
|
||||
"meta": cmeta,
|
||||
}
|
||||
return cache["layer"][key]
|
||||
|
||||
def _route_D_network_only(
|
||||
self,
|
||||
start_lat: float, start_lon: float,
|
||||
|
|
|
|||
157
backend/services/navi_offroute/tests/test_hpa_runtime.py
Normal file
157
backend/services/navi_offroute/tests/test_hpa_runtime.py
Normal file
|
|
@ -0,0 +1,157 @@
|
|||
"""HPA* two-level runtime tests (Phase H3). Abstract search + coverage/freshness fallback on
|
||||
synthetic tile DBs; dispatcher gating end-to-end. Refinement on live rasters is exercised in
|
||||
H4 ops / H5 path-quality, not here (these inputs are deterministic without the real builder)."""
|
||||
import logging
|
||||
import sqlite3
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from services.navi_offroute import astar, hpa_build as hb
|
||||
import services.navi_offroute.router as router_mod
|
||||
import services.navi_offroute.transitions as p_trans
|
||||
from services.navi_offroute.router import OffrouteRouter
|
||||
from services.navi_offroute.transitions import _latlon_to_pixel as _ll2px, _pixel_to_latlon as _px2ll
|
||||
|
||||
# Four-chunk region (0,0),(1,0),(0,1),(1,1) with a known path 0,0 -> 1,0 -> 1,1.
|
||||
_CHUNKS = [(0, 0), (0, 1), (1, 0), (1, 1)]
|
||||
# bounds = (south, north, west, east); spans chunks 0..1 in both axes.
|
||||
_BOUNDS = (0.001, hb.CHUNK_DEG + 0.001, 0.001, hb.CHUNK_DEG + 0.001)
|
||||
|
||||
|
||||
def _make_tile_db(path, rows, hash_ok=True):
|
||||
conn = sqlite3.connect(path)
|
||||
hb._init_schema(conn)
|
||||
conn.executemany(hb._INSERT, rows)
|
||||
h = astar._hpa_profile_hash() if hash_ok else "wrong-hash"
|
||||
conn.execute("INSERT OR REPLACE INTO meta (key, value) VALUES ('mode_profile_hash', ?)", (h,))
|
||||
conn.commit()
|
||||
conn.close()
|
||||
|
||||
|
||||
def _base_rows():
|
||||
# One trivial row per chunk so all four count as "present", plus the (1,0) hop 15->10
|
||||
# that carries the only non-seam cost on the 0,0 -> 1,0 -> 1,1 route.
|
||||
rows = [(cx, cy, 0, 0, 1, 99.0) for (cx, cy) in _CHUNKS]
|
||||
rows.append((1, 0, 0, 15, 10, 5.0)) # left entrance -> bottom entrance, foot, cost 5
|
||||
return rows
|
||||
|
||||
|
||||
def test_hpa_abstract_search_finds_path_through_chunks(tmp_path):
|
||||
db = str(tmp_path / "tiles.db")
|
||||
_make_tile_db(db, _base_rows())
|
||||
conn = sqlite3.connect(db)
|
||||
# START -> right entrance of (0,0); top entrance of (1,1) -> GOAL (both free pseudo-edges).
|
||||
start_edges = {(0, 0, 5, 0): 0.0}
|
||||
goal_edges = {(1, 1, 0, 0): 0.0}
|
||||
seq, cost = astar._hpa_abstract_search(conn, _CHUNKS, [0], start_edges, goal_edges)
|
||||
conn.close()
|
||||
assert seq is not None
|
||||
chunks_visited = [s[:2] for s in seq]
|
||||
assert chunks_visited[0] == (0, 0) and chunks_visited[-1] == (1, 1)
|
||||
# 0,0 -> (seam) 1,0 -> (intra 5s) -> (seam) 1,1 ; only the (1,0) intra row is non-free.
|
||||
assert cost == pytest.approx(5.0)
|
||||
assert (1, 0) in chunks_visited
|
||||
|
||||
|
||||
def test_hpa_falls_back_on_missing_chunk(tmp_path):
|
||||
db = str(tmp_path / "tiles.db")
|
||||
rows = [r for r in _base_rows() if not (r[0] == 1 and r[1] == 1)] # drop chunk (1,1)
|
||||
_make_tile_db(db, rows)
|
||||
idx, path, cost, reason = astar.astar_hpa_multimode(
|
||||
db, {"bounds": _BOUNDS}, 0.002, 0.002, hb.CHUNK_DEG + 0.0005, hb.CHUNK_DEG + 0.0005,
|
||||
np.array([0]), np.array([0]), "pragmatic", None)
|
||||
assert idx == -1 and reason == "missing_chunk"
|
||||
assert path.shape == (0, 3) and not np.isfinite(cost)
|
||||
|
||||
|
||||
def test_hpa_falls_back_on_stale_profile_hash(tmp_path):
|
||||
db = str(tmp_path / "tiles.db")
|
||||
_make_tile_db(db, _base_rows(), hash_ok=False)
|
||||
idx, path, cost, reason = astar.astar_hpa_multimode(
|
||||
db, {"bounds": _BOUNDS}, 0.002, 0.002, hb.CHUNK_DEG + 0.0005, hb.CHUNK_DEG + 0.0005,
|
||||
np.array([0]), np.array([0]), "pragmatic", None)
|
||||
assert idx == -1 and reason == "stale_profile"
|
||||
|
||||
|
||||
# ── dispatcher gating (end-to-end through _route_auto, stubbed readers) ───────
|
||||
|
||||
def _p_meta(rows, cols, cell_m=100.0):
|
||||
import math
|
||||
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, a): self._a = a
|
||||
def get_friction_grid(self, **k): return self._a
|
||||
def get_barrier_grid(self, **k): return self._a
|
||||
def get_trails_grid(self, **k): return self._a
|
||||
def get_wilderness_grid(self, **k): return self._a
|
||||
def close(self): pass
|
||||
|
||||
|
||||
class _StubDem:
|
||||
def __init__(self, e, m): self._e, self._m = e, m
|
||||
def get_elevation_grid(self, **k): return self._e, self._m
|
||||
def latlon_to_pixel(self, lat, lon, m): return _ll2px(lat, lon, m)
|
||||
def pixel_to_latlon(self, r, c, m): return _px2ll(r, c, m)
|
||||
def close(self): pass
|
||||
|
||||
|
||||
def _stub_router(monkeypatch):
|
||||
n = 20
|
||||
meta = _p_meta(n, n)
|
||||
r = OffrouteRouter()
|
||||
r.dem_reader = _StubDem(np.full((n, n), 1000.0), meta)
|
||||
fr = np.full((n, n), 30, dtype=np.uint8)
|
||||
r.friction_reader = _Grid(fr)
|
||||
r.barrier_reader = _Grid(np.zeros((n, n), np.uint8))
|
||||
r.trail_reader = _Grid(np.zeros((n, n), np.uint8))
|
||||
r.wilderness_reader = _Grid(np.zeros((n, n), np.uint8))
|
||||
monkeypatch.setattr(router_mod, "get_mvum_access_grid",
|
||||
lambda *a, **k: (_ for _ in ()).throw(RuntimeError("no mvum")))
|
||||
monkeypatch.setattr(p_trans, "load_parking_index",
|
||||
lambda *a, **k: type("I", (), {"query_parking_near_line": lambda s, c, buffer_m=2000: []})())
|
||||
monkeypatch.setattr(p_trans, "load_trailheads",
|
||||
lambda *a, **k: type("I", (), {"query_trailheads_near_line": lambda s, c, buffer_m=2000: []})())
|
||||
monkeypatch.setattr(p_trans, "get_surface_change_candidates", lambda *a, **k: [])
|
||||
monkeypatch.setattr(OffrouteRouter, "_spatial_eligible_modes",
|
||||
lambda self, lat, lon, cache: frozenset({"foot"}))
|
||||
s_lat, s_lon = _px2ll(5, 5, meta)
|
||||
e_lat, e_lon = _px2ll(15, 15, meta)
|
||||
return r, (s_lat, s_lon, e_lat, e_lon)
|
||||
|
||||
|
||||
def test_hpa_dispatcher_uses_hpa_when_tile_db_set(tmp_path, monkeypatch, caplog):
|
||||
db = str(tmp_path / "tiles.db")
|
||||
_make_tile_db(db, _base_rows()) # a real file so os.path.exists passes
|
||||
monkeypatch.setattr(router_mod, "HPA_TILE_DB", db)
|
||||
|
||||
calls = []
|
||||
|
||||
def spy(*a, **k):
|
||||
calls.append(True)
|
||||
return 0, np.array([[5, 5, 0], [6, 6, 0]], dtype=np.int64), 123.0, None
|
||||
monkeypatch.setattr(router_mod, "astar_hpa_multimode", spy)
|
||||
|
||||
r, (s_lat, s_lon, e_lat, e_lon) = _stub_router(monkeypatch)
|
||||
with caplog.at_level(logging.INFO, logger="navi_offroute.router"):
|
||||
out = r._route_auto(s_lat, s_lon, e_lat, e_lon, "pragmatic")
|
||||
assert out["status"] == "ok"
|
||||
assert calls == [True] # HPA was attempted + taken
|
||||
assert "auto: HPA*" in caplog.text
|
||||
|
||||
# strict boundary mode -> ineligible -> HPA not attempted, logged fallback, unified used.
|
||||
calls.clear()
|
||||
caplog.clear()
|
||||
r2, _ = _stub_router(monkeypatch)
|
||||
with caplog.at_level(logging.INFO, logger="navi_offroute.router"):
|
||||
out2 = r2._route_auto(s_lat, s_lon, e_lat, e_lon, "strict")
|
||||
assert out2["status"] == "ok"
|
||||
assert calls == [] # spy never called
|
||||
assert "HPA fallback reason=boundary_mode" in caplog.text
|
||||
Loading…
Add table
Add a link
Reference in a new issue