diff --git a/backend/services/navi_offroute/hpa_build.py b/backend/services/navi_offroute/hpa_build.py new file mode 100644 index 0000000..4993a41 --- /dev/null +++ b/backend/services/navi_offroute/hpa_build.py @@ -0,0 +1,307 @@ +"""HPA* offline precompute pipeline (spec HPA-SPEC.md §7, Phase H2). + +Standalone CLI: for every chunk a region's bbox covers, run small entrance-to-entrance A* +searches on the chunk's native-resolution cost grid and store the scalar costs into a +per-region SQLite (spec §6). No router/kernel wiring — H3 consumes these tiles. + + python -m services.navi_offroute.hpa_build \ + --region idaho --bbox 41.9,-117.3,49.1,-110.9 --output /mnt/nav/hpa/idaho.db + +Deliberate v1 scope / deviations (flag for spec amendment): +- BOTH directions stored, not `from < to`. The anisotropic slope model (signed Tobler / + Herzog / linear) makes uphill cost != downhill, so entrance-pair costs are NOT symmetric: + 20×19 = 380 ordered pairs per mode per chunk, not the 190 spec §4 implies. Recommend + amending §4/§6 to say "directed pairs". +- Costs are PURE TERRAIN (slope + WorldCover friction + trails + wilderness). Boundary-mode + rules (PAD-US barriers, MVUM closures) and network_affinity are NOT baked — they are + boundary-mode/request dependent and belong at query time / in the fallback (H3). The build + passes a zero barrier grid; wilderness impassability (mode-fixed, not boundary-dependent) + IS baked. +- Only the 20 border entrances (spec §4). Transition cells (entrances ≥20, spec §5) are NOT + built here: they need the TrailheadIndex/OSMParkingIndex runtime singletons, deferred to + H3 / a follow-up. Known v1 limitation. +- Chunk grid: fixed-degree plate-carrée pinned at lat=0/lon=0, step CHUNK_DEG = 1.5 km / 111 + (spec §3/§13). No cos(lat) correction — a per-cell cos would make chunk boundaries + latitude-dependent and break the deterministic global grid. Chunks are thus ~1.5 km tall + and (cos lat)·1.5 km wide; entrance cells scale to each chunk's actual fetched grid shape, + so the nominal "50×50" is not assumed. The §3 Z12-tile 5×5 alignment (an I/O optimization) + is deferred: v1 reads per chunk. +""" +import argparse +import hashlib +import math +import os +import sqlite3 +import subprocess +import sys +import time +from concurrent.futures import ProcessPoolExecutor +from datetime import datetime, timezone + +import numpy as np + +from shared.dem import DEMReader, dem_path +from .astar import astar_multigoal, inflate_cost_multiplier +from .barriers import WildernessReader, wilderness_tif_path +from .cost import MODE_PROFILES, compute_cost_multiplier_grid +from .friction import FrictionReader, friction_to_multiplier +from .trails import TrailReader + +MODE_ORDER = ["foot", "2w", "4w", "vehicle"] +_SPEED_FN_ID = {"tobler": 0, "herzog": 1, "linear": 2} + +# Deterministic chunk grid (spec §3/§13): plate-carrée, pinned at lat=0/lon=0. +CHUNK_KM = 1.5 +KM_PER_DEG = 111.0 +CHUNK_DEG = CHUNK_KM / KM_PER_DEG # ~0.013514 deg per chunk, both axes +ENTRANCES_PER_EDGE = 5 +EDGE_FRACTIONS = (1.0 / 6, 2.0 / 6, 3.0 / 6, 4.0 / 6, 5.0 / 6) # spec §4 + +_SCHEMA = """ +CREATE TABLE IF NOT EXISTS chunk_costs ( + chunk_x INTEGER NOT NULL, + chunk_y INTEGER NOT NULL, + mode_idx INTEGER NOT NULL, + from_entrance INTEGER NOT NULL, + to_entrance INTEGER NOT NULL, + cost_s REAL NOT NULL, + PRIMARY KEY (chunk_x, chunk_y, mode_idx, from_entrance, to_entrance) +); +CREATE INDEX IF NOT EXISTS idx_chunk_mode ON chunk_costs (chunk_x, chunk_y, mode_idx); +CREATE TABLE IF NOT EXISTS meta (key TEXT PRIMARY KEY, value TEXT); +""" +_INSERT = ("INSERT OR REPLACE INTO chunk_costs " + "(chunk_x, chunk_y, mode_idx, from_entrance, to_entrance, cost_s) VALUES (?,?,?,?,?,?)") + + +# ── deterministic chunk geometry ───────────────────────────────────────────── + +def chunk_coords(lat, lon): + """(chunk_x, chunk_y) for a lat/lon on the global plate-carrée grid pinned at 0/0.""" + return int(math.floor(lon / CHUNK_DEG)), int(math.floor(lat / CHUNK_DEG)) + + +def chunk_bounds(chunk_x, chunk_y): + """(south, west, north, east) degrees for a chunk.""" + return (chunk_y * CHUNK_DEG, chunk_x * CHUNK_DEG, + (chunk_y + 1) * CHUNK_DEG, (chunk_x + 1) * CHUNK_DEG) + + +def chunks_in_bbox(south, west, north, east): + """All (chunk_x, chunk_y) whose cells the bbox touches (inclusive).""" + cx0, cy0 = chunk_coords(south, west) + cx1, cy1 = chunk_coords(north, east) + return [(cx, cy) + for cx in range(min(cx0, cx1), max(cx0, cx1) + 1) + for cy in range(min(cy0, cy1), max(cy0, cy1) + 1)] + + +def _entrance_cells(rows, cols): + """The 20 border entrance (row, col) cells, indexed per spec §4: + 0..4 top L→R, 5..9 right T→B, 10..14 bottom L→R, 15..19 left T→B.""" + r_last, c_last = rows - 1, cols - 1 + cells = [] + cells += [(0, int(round(f * c_last))) for f in EDGE_FRACTIONS] # top + cells += [(int(round(f * r_last)), c_last) for f in EDGE_FRACTIONS] # right + cells += [(r_last, int(round(f * c_last))) for f in EDGE_FRACTIONS] # bottom + cells += [(int(round(f * r_last)), 0) for f in EDGE_FRACTIONS] # left + return cells + + +# ── per-chunk cost computation (pure: testable without readers/processes) ───── + +def compute_chunk_rows(chunk_x, chunk_y, elevation, friction_mult, friction_raw, + trails, wilderness, cell_size_m, modes=MODE_ORDER): + """Entrance-to-entrance directed costs for one chunk. Returns a list of + (chunk_x, chunk_y, mode_idx, from_entrance, to_entrance, cost_s) rows (finite only).""" + rows, cols = elevation.shape + if rows < 2 or cols < 2: + return [] + entrances = _entrance_cells(rows, cols) + elev = np.ascontiguousarray(elevation, dtype=np.float64) + barrier_grid = np.zeros((rows, cols), dtype=np.uint8) # barriers not baked (see docstring) + trail_grid = np.ascontiguousarray( + trails if trails is not None else np.zeros((rows, cols), np.uint8), dtype=np.uint8) + cs = float(cell_size_m) + out = [] + for mi, mode in enumerate(modes): + prof = MODE_PROFILES[mode] + cm = compute_cost_multiplier_grid( + elevation, cell_size_lat_m=cs, cell_size_lon_m=cs, + friction=friction_mult, friction_raw=friction_raw, wilderness=wilderness, mode=mode) + cm = np.ascontiguousarray(inflate_cost_multiplier(cm), dtype=np.float64) + tfl = np.full(256, np.inf, dtype=np.float64) + for tv, fric in prof.trail_friction.items(): + tfl[tv] = np.inf if fric is None else float(fric) + sfid = _SPEED_FN_ID.get(prof.speed_function, 0) + mg = float(np.tan(np.radians(prof.max_slope_deg))) + bspd = float(prof.base_speed_kmh) + for fi, (fr, fc) in enumerate(entrances): + for ti, (gr, gc) in enumerate(entrances): + if ti == fi: + continue + _, _, cost = astar_multigoal( + cm, elev, cs, cs, mg, sfid, bspd, trail_grid, tfl, barrier_grid, 1, + int(fr), int(fc), + np.array([gr], dtype=np.int64), np.array([gc], dtype=np.int64)) + if np.isfinite(cost): + out.append((chunk_x, chunk_y, mi, fi, ti, float(cost))) + return out + + +# ── reader-backed chunk fetch + process worker ─────────────────────────────── + +_READERS = {} + + +def _get_readers(): + """Lazily build the readers once per (worker) process.""" + if not _READERS: + _READERS["dem"] = DEMReader(dem_path()) + _READERS["friction"] = FrictionReader() + _READERS["trail"] = TrailReader() + _READERS["wild"] = WildernessReader() if wilderness_tif_path().exists() else None + return _READERS + + +def _fetch_chunk_rasters(chunk_x, chunk_y, readers): + """Fetch (elevation, friction_mult, friction_raw, trails, wilderness, cell_size_m) for a + chunk via the existing readers — same pattern as router._fetch_auto_rasters.""" + s, w, n, e = chunk_bounds(chunk_x, chunk_y) + elevation, meta = readers["dem"].get_elevation_grid(south=s, north=n, west=w, east=e) + shape = elevation.shape + friction_raw = readers["friction"].get_friction_grid( + south=s, north=n, west=w, east=e, target_shape=shape) + friction_mult = friction_to_multiplier(friction_raw) + trails = readers["trail"].get_trails_grid(south=s, north=n, west=w, east=e, target_shape=shape) + wilderness = None + if readers["wild"] is not None: + wilderness = readers["wild"].get_wilderness_grid( + south=s, north=n, west=w, east=e, target_shape=shape) + return elevation, friction_mult, friction_raw, trails, wilderness, float(meta["cell_size_m"]) + + +def _chunk_worker(chunk): + """ProcessPool worker: fetch + compute one chunk's rows. Errors -> empty (logged by caller).""" + cx, cy = chunk + try: + readers = _get_readers() + elev, fmult, fraw, trails, wild, cs = _fetch_chunk_rasters(cx, cy, readers) + return cx, cy, compute_chunk_rows(cx, cy, elev, fmult, fraw, trails, wild, cs), None + except Exception as exc: # pragma: no cover - exercised in H4 ops, not unit tests + return cx, cy, [], repr(exc) + + +# ── SQLite assembly ────────────────────────────────────────────────────────── + +def _init_schema(conn): + conn.executescript(_SCHEMA) + + +def _mode_profile_hash(): + return hashlib.sha256(repr(MODE_PROFILES).encode()).hexdigest() + + +def _dem_version(): + try: + return f"mtime:{int(os.path.getmtime(dem_path()))}" + except Exception: + return "unknown" + + +def _git_commit(): + try: + return subprocess.check_output( + ["git", "rev-parse", "HEAD"], stderr=subprocess.DEVNULL).decode().strip() + except Exception: + return "unknown" + + +def _write_meta(conn, region, bbox): + meta = { + "region_name": region, + "region_bbox": ",".join(str(x) for x in bbox), + "chunk_size_cells": "50", + "chunk_size_km": str(CHUNK_KM), + "chunk_deg": repr(CHUNK_DEG), + "entrances_per_edge": str(ENTRANCES_PER_EDGE), + "mode_profile_hash": _mode_profile_hash(), + "dem_version": _dem_version(), + "build_timestamp_utc": datetime.now(timezone.utc).isoformat(), + "build_git_commit": _git_commit(), + } + conn.executemany("INSERT OR REPLACE INTO meta (key, value) VALUES (?, ?)", list(meta.items())) + + +def build_region(region, bbox, output, max_workers=None, chunks_only=None): + """Build the cost-tile SQLite for `bbox`=(south, west, north, east). Returns a summary dict.""" + south, west, north, east = bbox + chunks = chunks_in_bbox(south, west, north, east) + if chunks_only is not None: + x0, y0, x1, y1 = chunks_only + chunks = [(cx, cy) for (cx, cy) in chunks + if min(x0, x1) <= cx <= max(x0, x1) and min(y0, y1) <= cy <= max(y0, y1)] + + conn = sqlite3.connect(output) + conn.execute("PRAGMA journal_mode=WAL") + conn.execute("PRAGMA synchronous=NORMAL") + _init_schema(conn) + _write_meta(conn, region, bbox) + conn.commit() + + t0 = time.perf_counter() + rows_total, failed = 0, [] + workers = max_workers or os.cpu_count() + with ProcessPoolExecutor(max_workers=workers) as ex: + for cx, cy, chunk_rows, err in ex.map(_chunk_worker, chunks): + if err is not None: + failed.append((cx, cy, err)) + continue + if chunk_rows: + conn.executemany(_INSERT, chunk_rows) + rows_total += len(chunk_rows) + conn.commit() + elapsed = time.perf_counter() - t0 + # Stored rows are finite only; the rest of the 380×modes×chunks possibilities were pruned. + possible = len(chunks) * len(MODE_ORDER) * 20 * 19 + summary = {"region": region, "chunks": len(chunks), "rows": rows_total, + "inf_pruned": possible - rows_total, "failed": len(failed), + "workers": workers, "elapsed_s": elapsed} + conn.execute("VACUUM") + conn.close() + print(f"HPA build [{region}]: {summary['chunks']} chunks, {rows_total} rows, " + f"{summary['inf_pruned']} inf-pruned, {len(failed)} failed, " + f"{workers} workers, {elapsed:.1f}s -> {output}") + for cx, cy, err in failed[:10]: + print(f" chunk ({cx},{cy}) failed: {err}", file=sys.stderr) + return summary + + +def _parse_csv_floats(s, n): + parts = [float(x) for x in s.split(",")] + if len(parts) != n: + raise argparse.ArgumentTypeError(f"expected {n} comma-separated values, got {len(parts)}") + return parts + + +def main(argv=None): + p = argparse.ArgumentParser( + prog="python -m services.navi_offroute.hpa_build", + description="Precompute HPA* chunk-cost tiles for a region (HPA-SPEC.md §7).") + p.add_argument("--region", required=True, help="region name, recorded in meta.region_name") + p.add_argument("--bbox", required=True, type=lambda s: _parse_csv_floats(s, 4), + metavar="S,W,N,E", help="region bbox: south,west,north,east (decimal degrees)") + p.add_argument("--output", required=True, help="path to the SQLite tile DB to create") + p.add_argument("--max-workers", type=int, default=os.cpu_count(), + help="ProcessPoolExecutor workers (default: os.cpu_count())") + p.add_argument("--chunks-only", type=lambda s: [int(x) for x in s.split(",")], + default=None, metavar="X1,Y1,X2,Y2", + help="debug: constrain the build to a chunk-coord bbox") + args = p.parse_args(argv) + south, west, north, east = args.bbox + build_region(args.region, (south, west, north, east), args.output, + max_workers=args.max_workers, chunks_only=args.chunks_only) + + +if __name__ == "__main__": + main() diff --git a/backend/services/navi_offroute/tests/test_hpa_build.py b/backend/services/navi_offroute/tests/test_hpa_build.py new file mode 100644 index 0000000..b3eeddc --- /dev/null +++ b/backend/services/navi_offroute/tests/test_hpa_build.py @@ -0,0 +1,82 @@ +"""HPA* precompute pipeline tests (Phase H2). Structural correctness on synthetic chunks; +no real DEM/readers, no ProcessPool — compute_chunk_rows is exercised directly in-process.""" +import math +import sqlite3 + +import numpy as np + +from services.navi_offroute import hpa_build as hb + + +def test_chunk_coords_deterministic(): + # Pure function of (lat, lon): identical on repeat, and matches the floor formula. + lat, lon = 43.7500, -114.9000 + assert hb.chunk_coords(lat, lon) == hb.chunk_coords(lat, lon) + assert hb.chunk_coords(lat, lon) == ( + int(math.floor(lon / hb.CHUNK_DEG)), int(math.floor(lat / hb.CHUNK_DEG))) + # Adjacent points one chunk-step apart land in adjacent chunks; bounds enclose the point. + cx, cy = hb.chunk_coords(lat, lon) + s, w, n, e = hb.chunk_bounds(cx, cy) + assert s <= lat < n and w <= lon < e + assert hb.chunk_coords(lat + hb.CHUNK_DEG, lon) == (cx, cy + 1) + + +def test_entrance_cells_layout(): + cells = hb._entrance_cells(50, 50) + assert len(cells) == 20 + assert all(0 <= r <= 49 and 0 <= c <= 49 for r, c in cells) + # Index convention (spec §4): 0..4 top, 5..9 right, 10..14 bottom, 15..19 left. + assert all(r == 0 for r, c in cells[0:5]) # top edge -> row 0 + assert all(c == 49 for r, c in cells[5:10]) # right edge -> col 49 + assert all(r == 49 for r, c in cells[10:15]) # bottom edge -> row 49 + assert all(c == 0 for r, c in cells[15:20]) # left edge -> col 0 + # Even spacing L→R / T→B along each edge (strictly increasing free coordinate). + assert [c for _, c in cells[0:5]] == sorted(c for _, c in cells[0:5]) + assert [r for r, _ in cells[5:10]] == sorted(r for r, _ in cells[5:10]) + # Fractions 1/6..5/6 of the 49-cell span. + assert [c for _, c in cells[0:5]] == [int(round(f * 49)) for f in hb.EDGE_FRACTIONS] + + +def _flat_chunk(n=50, fill_elev=1000.0): + elevation = np.full((n, n), fill_elev, dtype=np.float64) + friction_mult = np.ones((n, n), dtype=np.float64) + friction_raw = np.full((n, n), 30, dtype=np.uint8) # grass + trails = np.zeros((n, n), dtype=np.uint8) + wilderness = np.zeros((n, n), dtype=np.uint8) + return elevation, friction_mult, friction_raw, trails, wilderness + + +def _cost(conn, mode_idx, fr, to): + row = conn.execute( + "SELECT cost_s FROM chunk_costs WHERE chunk_x=0 AND chunk_y=0 AND mode_idx=? " + "AND from_entrance=? AND to_entrance=?", (mode_idx, fr, to)).fetchone() + return row[0] if row else None + + +def test_build_one_chunk_fixture(): + elev, fmult, fraw, trails, wild = _flat_chunk() + rows = hb.compute_chunk_rows(0, 0, elev, fmult, fraw, trails, wild, + cell_size_m=30.0, modes=("foot",)) + conn = sqlite3.connect(":memory:") + hb._init_schema(conn) + conn.executemany(hb._INSERT, rows) + + # Flat passable grass: every foot entrance pair routes (380 finite directed pairs). + assert len(rows) == 20 * 19 + # Distance monotonicity: along the top edge, 0→1 (adjacent) is cheaper than 0→4 (far). + assert _cost(conn, 0, 0, 1) < _cost(conn, 0, 0, 4) + # top-middle (2) -> bottom-middle (12): a straight vertical crossing. + flat_mid = _cost(conn, 0, 2, 12) + assert flat_mid is not None + + # A steep hill on the straight 2→12 column forces a detour / slope penalty -> costlier. + elev_hill = elev.copy() + elev_hill[20:30, 20:30] = 2600.0 # ~1600 m bump over ~300 m: cliff-steep + rows_h = hb.compute_chunk_rows(0, 0, elev_hill, fmult, fraw, trails, wild, + cell_size_m=30.0, modes=("foot",)) + conn_h = sqlite3.connect(":memory:") + hb._init_schema(conn_h) + conn_h.executemany(hb._INSERT, rows_h) + hill_mid = _cost(conn_h, 0, 2, 12) + assert hill_mid is not None + assert hill_mid > flat_mid # the hill is not free to cross