mirror of
https://github.com/zvx-echo6/navi.git
synced 2026-08-26 17:31:37 +00:00
feat(offroute): numba A* with anisotropic Tobler, exponentially-inflated cost grid (combined #17+#18)
Replace MCP_Geometric in _pathfind_wilderness with a numba-jit anisotropic A* (new astar.py): signed-slope speed (climbing != descending; tobler peaks at -0.05), hard cliff, per-edge avg context multiplier, trail-takes-both via 256-entry lookup, per-edge barriers (strict/pragmatic/emergency), multi-goal A* (first popped wins) with admissible distance/base-speed heuristic. New compute_cost_multiplier_grid (slope-free context multiplier) + exponential inflation (sigma=1.8; inf->HARD=50*p95 for blur, inf re-imposed). numba>=0.59 added (numba 0.65.1). fix: wilderness leg is always foot effort; mode parameter reserved for future flexibility. _pathfind_wilderness keeps the mode param (threaded from _route_A/B/C) but hardcodes cost_mode=foot for the cost grid, trail friction, speed function, base speed, and max grade. Off-trail math for MTB/ATV/vehicle is not well-grounded and real-world wilderness traversal is foot regardless (push the bike, walk past where the vehicle stops). User mode still drives entry-point eligibility (query_radius highway filter) and Valhalla network costing. Matches the original pre-#17 design: wilderness ALWAYS uses foot. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Matt
parent
f43b8a9ff8
commit
9d684bef43
5 changed files with 537 additions and 34 deletions
|
|
@ -26,6 +26,7 @@ dependencies = [
|
|||
"pmtiles>=3", # planet-DEM PMTiles reader
|
||||
# navi-offroute (extraction #8): off-network router + readers.
|
||||
"scikit-image>=0.22", # MCP_Geometric least-cost pathfinding (router.py)
|
||||
"numba>=0.59", # navi-offroute anisotropic A* JIT pathfinder (astar.py)
|
||||
"rasterio>=1.3", # barriers/wilderness/trails/friction raster readers
|
||||
"psutil>=5.9", # MEMORY_LIMIT_GB enforcement in router.py
|
||||
# scripts/overture_import.py: Overture Places ETL (S3 Parquet -> overture PG).
|
||||
|
|
|
|||
249
backend/services/navi_offroute/astar.py
Normal file
249
backend/services/navi_offroute/astar.py
Normal file
|
|
@ -0,0 +1,249 @@
|
|||
"""Numba-jit anisotropic A* for wilderness pathfinding (replaces MCP_Geometric).
|
||||
|
||||
Single-mode, multi-goal least-time pathfinder over a raster grid. Unlike the old
|
||||
isotropic MCP, the per-edge time is anisotropic: it depends on the *signed* slope
|
||||
between the two cells (climbing != descending) via the mode's speed function
|
||||
(signed Tobler / Herzog / linear), the average per-cell context multiplier of the
|
||||
two endpoints (or the trail friction when either endpoint is on a trail), and a
|
||||
barrier/boundary rule. The first goal cell popped from the open set wins, which is
|
||||
optimal under the admissible heuristic (straight-line distance / base speed).
|
||||
|
||||
Cliffs (|grade| > max_grade) are a hard wall here; smoothing is deferred to #19.
|
||||
"""
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
from numba import njit
|
||||
from scipy.ndimage import gaussian_filter
|
||||
|
||||
INF = np.inf
|
||||
|
||||
# Exponential-inflation parameters (see inflate_cost_multiplier).
|
||||
INFLATE_SIGMA = 1.8 # ~25% contribution at a 3-cell radius
|
||||
INFLATE_HARD_FACTOR = 50.0 # HARD sentinel = 50 x p95 of finite multipliers
|
||||
INFLATE_HARD_CAP = 1e12 # cap to avoid float overflow in the blur
|
||||
|
||||
|
||||
def inflate_cost_multiplier(mult, sigma=INFLATE_SIGMA):
|
||||
"""Exponentially inflate a context-cost multiplier grid so hard cells bleed a
|
||||
decaying penalty into their neighbours, giving A* a smooth gradient field to
|
||||
descend instead of hugging walls.
|
||||
|
||||
Impassable (inf) cells are replaced by a large finite HARD sentinel for the blur
|
||||
only, then restored to inf at their original positions so true impassability is
|
||||
preserved exactly. HARD = INFLATE_HARD_FACTOR * p95(finite), capped.
|
||||
"""
|
||||
inf_mask = ~np.isfinite(mult)
|
||||
finite = mult[~inf_mask]
|
||||
if finite.size == 0:
|
||||
return np.full(mult.shape, np.inf, dtype=np.float64)
|
||||
p95 = float(np.percentile(finite, 95))
|
||||
hard = min(INFLATE_HARD_FACTOR * p95, INFLATE_HARD_CAP)
|
||||
work = np.where(inf_mask, hard, mult).astype(np.float64)
|
||||
blurred = gaussian_filter(work, sigma=sigma, mode="nearest")
|
||||
blurred[inf_mask] = np.inf
|
||||
return blurred
|
||||
|
||||
|
||||
@njit(cache=True)
|
||||
def _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade):
|
||||
"""Mode speed (km/h) for a signed grade. Inlined per speed_function_id:
|
||||
0=tobler (signed; peaks at grade=-0.05), 1=herzog wheeled, 2=linear degrade."""
|
||||
if speed_function_id == 0:
|
||||
return 0.6 * base_speed_kmh * math.exp(-3.5 * abs(signed_grade + 0.05))
|
||||
elif speed_function_id == 1:
|
||||
s = signed_grade
|
||||
sa = abs(s)
|
||||
denom = (1337.8 * s**6 + 278.19 * s**5 - 517.39 * s**4
|
||||
- 78.199 * s**3 + 93.419 * s**2 + 19.825 * sa + 1.64)
|
||||
if denom < 0.1:
|
||||
denom = 0.1
|
||||
rel = 1.0 / denom
|
||||
if rel < 0.05:
|
||||
rel = 0.05
|
||||
elif rel > 1.5:
|
||||
rel = 1.5
|
||||
return base_speed_kmh * rel
|
||||
else:
|
||||
v = base_speed_kmh * (1.0 - abs(signed_grade) / max_grade)
|
||||
if v < 0.0:
|
||||
v = 0.0
|
||||
return v
|
||||
|
||||
|
||||
@njit(cache=True)
|
||||
def astar_multigoal(
|
||||
cost_mult, # 2D float64: per-cell context multiplier, post-inflation (inf=impassable)
|
||||
elevation, # 2D float64: metres (NaN = impassable)
|
||||
cell_size_lat_m, # float
|
||||
cell_size_lon_m, # float
|
||||
max_grade, # float: tan(max_slope)
|
||||
speed_function_id, # int: 0=tobler 1=herzog 2=linear
|
||||
base_speed_kmh, # float
|
||||
trail_grid, # 2D uint8: 0=none else trail value (5/15/25)
|
||||
trail_friction_lookup, # 1D float64 len 256: friction by trail value (inf=impassable)
|
||||
barrier_grid, # 2D uint8: 255=barrier
|
||||
boundary_mode_id, # int: 0=strict 1=pragmatic 2=emergency
|
||||
origin_row, origin_col,
|
||||
goal_rows, goal_cols, # 1D int arrays
|
||||
):
|
||||
"""A* from (origin_row,origin_col) to the nearest (by time) of the goals.
|
||||
Returns (best_goal_idx, path) where path is an int64 (N,2) array of (row,col)
|
||||
from origin to goal. (-1, empty) if no goal is reachable."""
|
||||
rows, cols = elevation.shape
|
||||
|
||||
goal_index = np.full((rows, cols), -1, dtype=np.int64)
|
||||
for gi in range(goal_rows.shape[0]):
|
||||
goal_index[goal_rows[gi], goal_cols[gi]] = gi
|
||||
|
||||
g_score = np.full((rows, cols), INF, dtype=np.float64)
|
||||
parent = np.full((rows, cols), -1, dtype=np.int64)
|
||||
closed = np.zeros((rows, cols), dtype=np.bool_)
|
||||
|
||||
# Binary min-heap (lazy deletion): parallel id/priority arrays.
|
||||
cap = rows * cols * 4
|
||||
if cap < 1024:
|
||||
cap = 1024
|
||||
heap_id = np.empty(cap, dtype=np.int64)
|
||||
heap_f = np.empty(cap, dtype=np.float64)
|
||||
hsize = 0
|
||||
|
||||
def heuristic(r, c):
|
||||
best = INF
|
||||
for gi in range(goal_rows.shape[0]):
|
||||
dr = (r - goal_rows[gi]) * cell_size_lat_m
|
||||
dc = (c - goal_cols[gi]) * cell_size_lon_m
|
||||
d = math.sqrt(dr * dr + dc * dc)
|
||||
if d < best:
|
||||
best = d
|
||||
return best * 3.6 / base_speed_kmh # metres -> seconds at base speed
|
||||
|
||||
# Seed origin.
|
||||
g_score[origin_row, origin_col] = 0.0
|
||||
heap_id[0] = origin_row * cols + origin_col
|
||||
heap_f[0] = heuristic(origin_row, origin_col)
|
||||
hsize = 1
|
||||
|
||||
while hsize > 0:
|
||||
# Pop min.
|
||||
cur_id = heap_id[0]
|
||||
hsize -= 1
|
||||
heap_id[0] = heap_id[hsize]
|
||||
heap_f[0] = heap_f[hsize]
|
||||
i = 0
|
||||
while True:
|
||||
l = 2 * i + 1
|
||||
r = 2 * i + 2
|
||||
sm = i
|
||||
if l < hsize and heap_f[l] < heap_f[sm]:
|
||||
sm = l
|
||||
if r < hsize and heap_f[r] < heap_f[sm]:
|
||||
sm = r
|
||||
if sm != i:
|
||||
tid = heap_id[i]; heap_id[i] = heap_id[sm]; heap_id[sm] = tid
|
||||
tf = heap_f[i]; heap_f[i] = heap_f[sm]; heap_f[sm] = tf
|
||||
i = sm
|
||||
else:
|
||||
break
|
||||
|
||||
cr = cur_id // cols
|
||||
cc = cur_id % cols
|
||||
if closed[cr, cc]:
|
||||
continue # stale heap entry
|
||||
closed[cr, cc] = True
|
||||
|
||||
if goal_index[cr, cc] >= 0:
|
||||
# First goal popped is optimal — trace back.
|
||||
length = 1
|
||||
node = cur_id
|
||||
while parent[node // cols, node % cols] != -1:
|
||||
length += 1
|
||||
node = parent[node // cols, node % cols]
|
||||
path = np.empty((length, 2), dtype=np.int64)
|
||||
node = cur_id
|
||||
k = length - 1
|
||||
while node != -1:
|
||||
pr = node // cols
|
||||
pc = node % cols
|
||||
path[k, 0] = pr
|
||||
path[k, 1] = pc
|
||||
k -= 1
|
||||
node = parent[pr, pc]
|
||||
return goal_index[cr, cc], path, g_score[cr, cc]
|
||||
|
||||
g_cur = g_score[cr, cc]
|
||||
elev_cur = elevation[cr, cc]
|
||||
if math.isnan(elev_cur):
|
||||
continue
|
||||
|
||||
for dr in range(-1, 2):
|
||||
for dc in range(-1, 2):
|
||||
if dr == 0 and dc == 0:
|
||||
continue
|
||||
nr = cr + dr
|
||||
nc = cc + dc
|
||||
if nr < 0 or nr >= rows or nc < 0 or nc >= cols:
|
||||
continue
|
||||
if closed[nr, nc]:
|
||||
continue
|
||||
elev_n = elevation[nr, nc]
|
||||
if math.isnan(elev_n):
|
||||
continue
|
||||
|
||||
dlat = dr * cell_size_lat_m
|
||||
dlon = dc * cell_size_lon_m
|
||||
dist = math.sqrt(dlat * dlat + dlon * dlon)
|
||||
signed_grade = (elev_n - elev_cur) / dist
|
||||
if abs(signed_grade) > max_grade:
|
||||
continue # hard cliff
|
||||
spd = _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade)
|
||||
if spd <= 1e-9:
|
||||
continue
|
||||
base_time = dist * 3.6 / spd
|
||||
|
||||
tv_cur = trail_grid[cr, cc]
|
||||
tv_n = trail_grid[nr, nc]
|
||||
if tv_cur > 0 or tv_n > 0:
|
||||
# Trail-takes-both: pick the lower-friction trail cell.
|
||||
fc = trail_friction_lookup[tv_cur] if tv_cur > 0 else INF
|
||||
fn = trail_friction_lookup[tv_n] if tv_n > 0 else INF
|
||||
tf = fc if fc < fn else fn
|
||||
if not (tf < INF):
|
||||
continue # impassable trail for this mode
|
||||
edge = base_time * tf
|
||||
else:
|
||||
mc = cost_mult[cr, cc]
|
||||
mn = cost_mult[nr, nc]
|
||||
if (not (mc < INF)) or (not (mn < INF)):
|
||||
continue # impassable terrain (incl. wilderness)
|
||||
edge = base_time * 0.5 * (mc + mn)
|
||||
|
||||
if boundary_mode_id == 0: # strict
|
||||
if barrier_grid[cr, cc] == 255 or barrier_grid[nr, nc] == 255:
|
||||
continue
|
||||
elif boundary_mode_id == 1: # pragmatic
|
||||
if barrier_grid[cr, cc] == 255 or barrier_grid[nr, nc] == 255:
|
||||
edge *= 5.0
|
||||
# emergency (2): ignore barriers
|
||||
|
||||
tentative = g_cur + edge
|
||||
if tentative < g_score[nr, nc]:
|
||||
g_score[nr, nc] = tentative
|
||||
parent[nr, nc] = cur_id
|
||||
f = tentative + heuristic(nr, nc)
|
||||
if hsize < cap:
|
||||
# push + sift-up
|
||||
heap_id[hsize] = nr * cols + nc
|
||||
heap_f[hsize] = f
|
||||
j = hsize
|
||||
hsize += 1
|
||||
while j > 0:
|
||||
par = (j - 1) // 2
|
||||
if heap_f[j] < heap_f[par]:
|
||||
tid = heap_id[j]; heap_id[j] = heap_id[par]; heap_id[par] = tid
|
||||
tf2 = heap_f[j]; heap_f[j] = heap_f[par]; heap_f[par] = tf2
|
||||
j = par
|
||||
else:
|
||||
break
|
||||
|
||||
return -1, np.empty((0, 2), dtype=np.int64), INF
|
||||
|
|
@ -203,6 +203,91 @@ PRAGMATIC_BARRIER_MULTIPLIER = 5.0
|
|||
# COST GRID COMPUTATION
|
||||
# ═══════════════════════════════════════════════════════════════════════════════
|
||||
|
||||
def _per_cell_grade(elevation, cell_size_lat_m, cell_size_lon_m):
|
||||
"""Per-cell slope magnitude (rise/run), matching compute_cost_grid's gradient.
|
||||
Used only to classify the vehicle off-trail flat-field special case."""
|
||||
grade = np.zeros(elevation.shape, dtype=np.float32)
|
||||
dy = np.zeros(elevation.shape, dtype=np.float32)
|
||||
dy[1:-1, :] = ((elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m)) ** 2
|
||||
dy[0, :] = ((elevation[0, :] - elevation[1, :]) / cell_size_lat_m) ** 2
|
||||
dy[-1, :] = ((elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m) ** 2
|
||||
dy[:, 1:-1] += ((elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m)) ** 2
|
||||
dy[:, 0] += ((elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m) ** 2
|
||||
dy[:, -1] += ((elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m) ** 2
|
||||
np.sqrt(dy, out=grade)
|
||||
return grade
|
||||
|
||||
|
||||
def compute_cost_multiplier_grid(
|
||||
elevation: np.ndarray,
|
||||
cell_size_lat_m: float,
|
||||
cell_size_lon_m: float,
|
||||
friction: Optional[np.ndarray] = None,
|
||||
friction_raw: Optional[np.ndarray] = None,
|
||||
wilderness: Optional[np.ndarray] = None,
|
||||
mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot",
|
||||
) -> np.ndarray:
|
||||
"""Per-cell SLOPE-FREE context cost multiplier for the anisotropic A* pathfinder.
|
||||
|
||||
Returns a float64 grid: 1.0 baseline, higher = harder, np.inf = impassable. It
|
||||
combines the base WorldCover friction, the mode's terrain-friction overrides, the
|
||||
vehicle off-trail flat-field special case, and wilderness impassability. Slope,
|
||||
trails, and barriers are handled PER-EDGE in the A* kernel and are NOT applied here.
|
||||
|
||||
This is a strict refactor of compute_cost_grid's friction-and-mode pieces.
|
||||
(elevation + cell sizes are needed only for the vehicle flat-field slope check —
|
||||
that classification is per-cell, distinct from the per-edge traversal slope.)
|
||||
"""
|
||||
if mode not in MODE_PROFILES:
|
||||
raise ValueError(f"mode must be one of {list(MODE_PROFILES.keys())}")
|
||||
profile = MODE_PROFILES[mode]
|
||||
|
||||
mult = np.ones(elevation.shape, dtype=np.float64)
|
||||
|
||||
# Base WorldCover friction.
|
||||
if friction is not None:
|
||||
if friction.shape != elevation.shape:
|
||||
raise ValueError("Friction shape mismatch")
|
||||
np.multiply(mult, friction, out=mult)
|
||||
|
||||
# NaN elevation -> impassable.
|
||||
mult[np.isnan(elevation)] = np.inf
|
||||
|
||||
# Mode-specific terrain friction overrides.
|
||||
if friction_raw is not None and profile.terrain_friction_override:
|
||||
if friction_raw.shape != elevation.shape:
|
||||
raise ValueError("Friction_raw shape mismatch")
|
||||
for wc_class, override in profile.terrain_friction_override.items():
|
||||
if override is None:
|
||||
continue
|
||||
if override == np.inf:
|
||||
np.putmask(mult, friction_raw == wc_class, np.inf)
|
||||
else:
|
||||
m = friction_raw == wc_class
|
||||
mult[m] *= override
|
||||
del m
|
||||
|
||||
# Vehicle off-trail flat-field special case (overrides the inf set above on flat
|
||||
# grassland/cropland). Uses a per-cell slope classification.
|
||||
if mode == "vehicle" and profile.off_trail_flat_threshold_deg > 0 and friction_raw is not None:
|
||||
grade = _per_cell_grade(elevation, cell_size_lat_m, cell_size_lon_m)
|
||||
slope_deg = np.degrees(np.arctan(grade))
|
||||
flat_field = (
|
||||
(slope_deg <= profile.off_trail_flat_threshold_deg)
|
||||
& ((friction_raw == 30) | (friction_raw == 40))
|
||||
)
|
||||
mult[flat_field] = profile.off_trail_flat_friction
|
||||
del grade, slope_deg, flat_field
|
||||
|
||||
# Wilderness areas (mode-specific).
|
||||
if wilderness is not None and profile.wilderness_impassable:
|
||||
if wilderness.shape != elevation.shape:
|
||||
raise ValueError("Wilderness shape mismatch")
|
||||
np.putmask(mult, wilderness == 255, np.inf)
|
||||
|
||||
return mult
|
||||
|
||||
|
||||
def compute_cost_grid(
|
||||
elevation: np.ndarray,
|
||||
cell_size_m: float,
|
||||
|
|
|
|||
|
|
@ -31,10 +31,10 @@ import requests
|
|||
import psycopg2
|
||||
import psycopg2.extras
|
||||
from shapely.geometry import LineString
|
||||
from skimage.graph import MCP_Geometric
|
||||
from .astar import astar_multigoal, inflate_cost_multiplier
|
||||
|
||||
from shared.dem import DEMReader, dem_path
|
||||
from .cost import compute_cost_grid
|
||||
from .cost import compute_cost_grid, compute_cost_multiplier_grid, MODE_PROFILES
|
||||
from .friction import FrictionReader, friction_to_multiplier
|
||||
from .barriers import BarrierReader, WildernessReader, wilderness_tif_path
|
||||
from .trails import TrailReader
|
||||
|
|
@ -956,7 +956,7 @@ class OffrouteRouter:
|
|||
# Run wilderness pathfinding
|
||||
wilderness_result = self._pathfind_wilderness(
|
||||
start_lat, start_lon, end_lat, end_lon,
|
||||
entry_points, boundary_mode, "start"
|
||||
entry_points, boundary_mode, "start", mode=mode
|
||||
)
|
||||
|
||||
if wilderness_result.get("status") == "error":
|
||||
|
|
@ -1036,7 +1036,7 @@ class OffrouteRouter:
|
|||
# Run wilderness pathfinding FROM END toward entry points
|
||||
wilderness_result = self._pathfind_wilderness(
|
||||
end_lat, end_lon, start_lat, start_lon,
|
||||
entry_points, boundary_mode, "end"
|
||||
entry_points, boundary_mode, "end", mode=mode
|
||||
)
|
||||
|
||||
if wilderness_result.get("status") == "error":
|
||||
|
|
@ -1120,7 +1120,7 @@ class OffrouteRouter:
|
|||
# Phase 1: Wilderness pathfinding from START
|
||||
wilderness_start_result = self._pathfind_wilderness(
|
||||
start_lat, start_lon, end_lat, end_lon,
|
||||
entry_points_start, boundary_mode, "start"
|
||||
entry_points_start, boundary_mode, "start", mode=mode
|
||||
)
|
||||
|
||||
if wilderness_start_result.get("status") == "error":
|
||||
|
|
@ -1134,7 +1134,7 @@ class OffrouteRouter:
|
|||
# Phase 2: Wilderness pathfinding from END (run after freeing phase 1 memory)
|
||||
wilderness_end_result = self._pathfind_wilderness(
|
||||
end_lat, end_lon, start_lat, start_lon,
|
||||
entry_points_end, boundary_mode, "end"
|
||||
entry_points_end, boundary_mode, "end", mode=mode
|
||||
)
|
||||
|
||||
if wilderness_end_result.get("status") == "error":
|
||||
|
|
@ -1177,7 +1177,8 @@ class OffrouteRouter:
|
|||
dest_lat: float, dest_lon: float,
|
||||
entry_points: List[Dict],
|
||||
boundary_mode: str,
|
||||
label: str
|
||||
label: str,
|
||||
mode: str = "foot",
|
||||
) -> Dict:
|
||||
"""
|
||||
Run MCP wilderness pathfinding from origin toward entry points.
|
||||
|
|
@ -1258,24 +1259,52 @@ class OffrouteRouter:
|
|||
target_shape=elevation.shape
|
||||
)
|
||||
|
||||
# Compute cost grid (ALWAYS foot mode for wilderness)
|
||||
cost = compute_cost_grid(
|
||||
# ── Anisotropic A* pathfinding (replaces isotropic MCP) ──
|
||||
# Wilderness pathfinding ALWAYS uses foot effort, regardless of the user's mode.
|
||||
# The off-trail cost math for MTB/ATV/vehicle is not well-grounded (no peer-reviewed
|
||||
# off-road model), and real-world wilderness traversal is foot anyway: you push the
|
||||
# bike and walk past where the vehicle stops. The user's mode still affects entry-point
|
||||
# eligibility (the highway filter at the query_radius call sites) and the Valhalla
|
||||
# network-leg costing -- just not this wilderness leg.
|
||||
_ = mode # reserved for future flexibility; unused for wilderness cost
|
||||
cost_mode = "foot"
|
||||
profile = MODE_PROFILES[cost_mode]
|
||||
cell_size_m = meta["cell_size_m"]
|
||||
|
||||
# Wilderness grid only matters for modes that treat it as impassable.
|
||||
wilderness = None
|
||||
if profile.wilderness_impassable and 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=elevation.shape,
|
||||
)
|
||||
|
||||
# Per-cell context multiplier (slope-free; trails/barriers are per-edge in A*),
|
||||
# then exponentially inflated so hard cells bleed a decaying penalty outward.
|
||||
cost_mult = compute_cost_multiplier_grid(
|
||||
elevation,
|
||||
cell_size_m=meta["cell_size_m"],
|
||||
cell_size_lat_m=cell_size_m,
|
||||
cell_size_lon_m=cell_size_m,
|
||||
friction=friction_mult,
|
||||
friction_raw=friction_raw,
|
||||
trails=trails,
|
||||
barriers=barriers,
|
||||
wilderness=None,
|
||||
mvum=None,
|
||||
boundary_mode=boundary_mode,
|
||||
mode="foot",
|
||||
wilderness=wilderness,
|
||||
mode=cost_mode,
|
||||
)
|
||||
cost_mult = inflate_cost_multiplier(cost_mult)
|
||||
|
||||
# Free intermediate arrays
|
||||
del friction_mult, friction_raw
|
||||
gc.collect()
|
||||
|
||||
# Trail friction lookup (length-256, indexed by trail value; inf = impassable).
|
||||
trail_friction_lookup = np.full(256, np.inf, dtype=np.float64)
|
||||
for tv, fric in profile.trail_friction.items():
|
||||
trail_friction_lookup[tv] = np.inf if fric is None else float(fric)
|
||||
|
||||
speed_function_id = {"tobler": 0, "herzog": 1, "linear": 2}.get(profile.speed_function, 0)
|
||||
max_grade = float(np.tan(np.radians(profile.max_slope_deg)))
|
||||
|
||||
# Convert origin to pixel coordinates
|
||||
origin_row, origin_col = self.dem_reader.latlon_to_pixel(origin_lat, origin_lon, meta)
|
||||
|
||||
|
|
@ -1283,7 +1312,7 @@ class OffrouteRouter:
|
|||
if not (0 <= origin_row < rows and 0 <= origin_col < cols):
|
||||
return {"status": "error", "message": f"{label.capitalize()} point outside grid bounds"}
|
||||
|
||||
# Map entry points to pixels
|
||||
# Map entry points to pixels (these are the A* goals).
|
||||
entry_pixels = []
|
||||
for ep in entry_points:
|
||||
row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta)
|
||||
|
|
@ -1293,28 +1322,29 @@ class OffrouteRouter:
|
|||
if not entry_pixels:
|
||||
return {"status": "error", "message": f"No entry points map to grid bounds for {label}"}
|
||||
|
||||
# Run MCP
|
||||
mcp = MCP_Geometric(cost, fully_connected=True)
|
||||
cumulative_costs, traceback = mcp.find_costs([(origin_row, origin_col)])
|
||||
goal_rows = np.array([ep["row"] for ep in entry_pixels], dtype=np.int64)
|
||||
goal_cols = np.array([ep["col"] for ep in entry_pixels], dtype=np.int64)
|
||||
boundary_mode_id = {"strict": 0, "pragmatic": 1, "emergency": 2}.get(boundary_mode, 1)
|
||||
|
||||
# Find nearest reachable entry point
|
||||
best_entry = None
|
||||
best_cost = np.inf
|
||||
# Run multi-goal A* (first goal popped wins).
|
||||
elevation = np.ascontiguousarray(elevation, dtype=np.float64)
|
||||
best_goal_idx, path_indices, best_cost = astar_multigoal(
|
||||
cost_mult, elevation,
|
||||
float(cell_size_m), float(cell_size_m),
|
||||
max_grade, speed_function_id, float(profile.base_speed_kmh),
|
||||
np.ascontiguousarray(trails, dtype=np.uint8), trail_friction_lookup,
|
||||
np.ascontiguousarray(barriers, dtype=np.uint8), boundary_mode_id,
|
||||
int(origin_row), int(origin_col),
|
||||
goal_rows, goal_cols,
|
||||
)
|
||||
|
||||
for ep in entry_pixels:
|
||||
ep_cost = cumulative_costs[ep["row"], ep["col"]]
|
||||
if ep_cost < best_cost:
|
||||
best_cost = ep_cost
|
||||
best_entry = ep
|
||||
|
||||
if best_entry is None or np.isinf(best_cost):
|
||||
if best_goal_idx < 0 or len(path_indices) == 0 or np.isinf(best_cost):
|
||||
return {
|
||||
"status": "error",
|
||||
"message": f"No path found from {label} to any entry point (blocked by impassable terrain)"
|
||||
}
|
||||
|
||||
# Traceback path
|
||||
path_indices = mcp.traceback((best_entry["row"], best_entry["col"]))
|
||||
best_entry = entry_pixels[best_goal_idx]
|
||||
|
||||
# Convert to coordinates and collect stats
|
||||
coords = []
|
||||
|
|
@ -1350,7 +1380,7 @@ class OffrouteRouter:
|
|||
on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0
|
||||
|
||||
# Free memory
|
||||
del mcp, cumulative_costs, traceback, cost, trails, barriers, elevation
|
||||
del cost_mult, trails, barriers, elevation
|
||||
gc.collect()
|
||||
|
||||
return {
|
||||
|
|
|
|||
|
|
@ -559,3 +559,141 @@ def test_query_radius_soft_cap_filters_beyond_radius(monkeypatch):
|
|||
idx = object.__new__(EntryPointIndex)
|
||||
out = idx.query_radius(44.1, -115.0, 50, limit=10) # 50 km = 50000 m cap
|
||||
assert [r["id"] for r in out] == [1, 2]
|
||||
|
||||
|
||||
# ── Anisotropic A* pathfinder (#17+#18) ───────────────────────────────────
|
||||
import numpy as _np
|
||||
import services.navi_offroute.router as _router_mod
|
||||
from services.navi_offroute.astar import (
|
||||
_speed_kmh, astar_multigoal, inflate_cost_multiplier,
|
||||
)
|
||||
from services.navi_offroute.cost import compute_cost_multiplier_grid
|
||||
|
||||
_MG = float(_np.tan(_np.radians(40.0)))
|
||||
|
||||
|
||||
def test_signed_tobler_asymmetry():
|
||||
# Same magnitude, opposite sign -> downhill faster than uphill; peak near -0.05.
|
||||
up = _speed_kmh(0.2, 0, 6.0, _MG)
|
||||
down = _speed_kmh(-0.2, 0, 6.0, _MG)
|
||||
assert down > up
|
||||
peak = _speed_kmh(-0.05, 0, 6.0, _MG)
|
||||
assert peak >= _speed_kmh(0.0, 0, 6.0, _MG)
|
||||
assert peak >= _speed_kmh(-0.15, 0, 6.0, _MG)
|
||||
|
||||
|
||||
def test_inflation_bumps_neighbors_and_preserves_inf():
|
||||
grid = _np.ones((30, 30), dtype=_np.float64)
|
||||
grid[15, 15] = 100.0 # high finite cost
|
||||
out = inflate_cost_multiplier(grid)
|
||||
assert out[15, 16] > 1.0 # neighbor inflated
|
||||
assert out[0, 0] < 1.05 # far corner ~baseline
|
||||
|
||||
grid2 = _np.ones((30, 30), dtype=_np.float64)
|
||||
grid2[15, 15] = _np.inf # impassable
|
||||
out2 = inflate_cost_multiplier(grid2)
|
||||
assert _np.isinf(out2[15, 15]) # inf preserved exactly
|
||||
assert _np.isfinite(out2[15, 16]) and out2[15, 16] > 1.0 # neighbor bumped, not inf
|
||||
|
||||
|
||||
def _flat_inputs(n):
|
||||
elev = _np.zeros((n, n), dtype=_np.float64)
|
||||
mult = _np.ones((n, n), dtype=_np.float64)
|
||||
trail = _np.zeros((n, n), dtype=_np.uint8)
|
||||
lookup = _np.full(256, _np.inf, dtype=_np.float64)
|
||||
barr = _np.zeros((n, n), dtype=_np.uint8)
|
||||
return elev, mult, trail, lookup, barr
|
||||
|
||||
|
||||
def test_astar_small_synthetic_shortest_path():
|
||||
elev, mult, trail, lookup, barr = _flat_inputs(30)
|
||||
gr = _np.array([29], dtype=_np.int64)
|
||||
gc = _np.array([29], dtype=_np.int64)
|
||||
idx, path, cost = astar_multigoal(
|
||||
mult, elev, 30.0, 30.0, _MG, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc)
|
||||
assert idx == 0
|
||||
assert tuple(path[0]) == (0, 0)
|
||||
assert tuple(path[-1]) == (29, 29)
|
||||
assert len(path) == 30 # pure diagonal on a flat grid
|
||||
assert cost > 0 and _np.isfinite(cost)
|
||||
|
||||
|
||||
def test_astar_multigoal_picks_cheaper():
|
||||
elev, mult, trail, lookup, barr = _flat_inputs(30)
|
||||
gr = _np.array([0, 20], dtype=_np.int64) # goal0 at (0,5) near; goal1 at (20,20) far
|
||||
gc = _np.array([5, 20], dtype=_np.int64)
|
||||
idx, path, cost = astar_multigoal(
|
||||
mult, elev, 30.0, 30.0, _MG, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc)
|
||||
assert idx == 0
|
||||
assert tuple(path[-1]) == (0, 5)
|
||||
|
||||
|
||||
def test_compute_cost_multiplier_grid_math():
|
||||
elev = _np.zeros((4, 4), dtype=_np.float64)
|
||||
friction = _np.full((4, 4), 2.0, dtype=_np.float64)
|
||||
# mtb override: grass(30)=2.0, water(80)=inf
|
||||
fr = _np.full((4, 4), 30, dtype=_np.uint8)
|
||||
fr[0, 0] = 80
|
||||
mult = compute_cost_multiplier_grid(
|
||||
elev, 30.0, 30.0, friction=friction, friction_raw=fr, wilderness=None, mode="mtb")
|
||||
assert mult[1, 1] == 4.0 # 2.0 friction * 2.0 grass override
|
||||
assert _np.isinf(mult[0, 0]) # water impassable
|
||||
|
||||
|
||||
# ── _pathfind_wilderness mode wiring (mtb profile -> herzog + mtb trail set) ──
|
||||
|
||||
class _FakeDEM:
|
||||
def get_elevation_grid(self, south, north, west, east):
|
||||
return _np.zeros((10, 10), dtype=_np.float64), {"cell_size_m": 30.0}
|
||||
def latlon_to_pixel(self, lat, lon, meta):
|
||||
return (0, 0) if lat == 44.0 else (9, 9)
|
||||
def pixel_to_latlon(self, row, col, meta):
|
||||
return (44.0 + row * 0.001, -115.0 + col * 0.001)
|
||||
|
||||
|
||||
class _FakeGrid:
|
||||
def __init__(self, val, dtype):
|
||||
self.val, self.dtype = val, dtype
|
||||
def _grid(self, **k):
|
||||
return _np.full((10, 10), self.val, dtype=self.dtype)
|
||||
|
||||
|
||||
def test_pathfind_wilderness_always_uses_foot_effort(monkeypatch):
|
||||
# Even when called with mode="mtb", the wilderness cost is computed as foot:
|
||||
# compute_cost_multiplier_grid receives mode="foot", and A* gets the foot speed
|
||||
# function (tobler=0), foot base speed (6.0), and foot trail friction.
|
||||
captured = {}
|
||||
|
||||
def fake_mult(elevation, cell_size_lat_m, cell_size_lon_m,
|
||||
friction=None, friction_raw=None, wilderness=None, mode="foot"):
|
||||
captured["mult_mode"] = mode
|
||||
return _np.ones((10, 10), dtype=_np.float64)
|
||||
|
||||
def fake_astar(cost_mult, elevation, clat, clon, max_grade, sfid, base, trails,
|
||||
lookup, barriers, bmid, orow, ocol, grows, gcols):
|
||||
captured["speed_function_id"] = sfid
|
||||
captured["base_speed"] = base
|
||||
captured["lookup"] = lookup
|
||||
return 0, _np.array([[0, 0], [9, 9]], dtype=_np.int64), 6.0
|
||||
|
||||
monkeypatch.setattr(_router_mod, "compute_cost_multiplier_grid", fake_mult)
|
||||
monkeypatch.setattr(_router_mod, "astar_multigoal", fake_astar)
|
||||
monkeypatch.setattr(OffrouteRouter, "_init_readers", lambda self: None)
|
||||
|
||||
r = object.__new__(OffrouteRouter)
|
||||
r.dem_reader = _FakeDEM()
|
||||
r.friction_reader = type("F", (), {"get_friction_grid": lambda self, **k: _np.full((10, 10), 30, dtype=_np.uint8)})()
|
||||
r.barrier_reader = type("B", (), {"get_barrier_grid": lambda self, **k: _np.zeros((10, 10), dtype=_np.uint8)})()
|
||||
r.trail_reader = type("T", (), {"get_trails_grid": lambda self, **k: _np.zeros((10, 10), dtype=_np.uint8)})()
|
||||
r.wilderness_reader = None # foot is not wilderness_impassable -> not loaded anyway
|
||||
|
||||
ep = [{"lat": 44.001, "lon": -115.001, "highway_class": "track", "name": "t", "land_status": "open"}]
|
||||
out = r._pathfind_wilderness(44.0, -115.0, 44.001, -115.001, ep, "pragmatic", "start", mode="mtb")
|
||||
|
||||
assert out["status"] == "ok"
|
||||
assert captured["mult_mode"] == "foot" # cost grid built as foot despite mode=mtb
|
||||
assert captured["speed_function_id"] == 0 # tobler (foot)
|
||||
assert captured["base_speed"] == 6.0 # foot base speed
|
||||
assert captured["lookup"][5] == 0.1 # foot road
|
||||
assert captured["lookup"][15] == 0.3 # foot track
|
||||
assert captured["lookup"][25] == 0.5 # foot foot-trail
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue