diff --git a/backend/services/navi_offroute/astar.py b/backend/services/navi_offroute/astar.py index 0ba2374..39e3f00 100644 --- a/backend/services/navi_offroute/astar.py +++ b/backend/services/navi_offroute/astar.py @@ -263,3 +263,313 @@ def astar_multigoal( break return -1, np.empty((0, 2), dtype=np.int64), INF + + +# ═══════════════════════════════════════════════════════════════════════════════ +# MULTI-MODE A* (unified-graph, spec §2.3 / §10 / §11) +# ═══════════════════════════════════════════════════════════════════════════════ +# +# astar_multigoal_multimode extends the single-mode search to a (row, col, mode) +# state space: mode is part of the state, and mode-switching transition edges +# (parking lots, trailheads, road termini, surface boundaries) let the optimizer +# decide WHERE a mode change happens instead of a fixed leg ordering. Single-mode +# astar_multigoal above is unchanged and still serves explicit-mode requests; this +# kernel is invoked only by Auto (wired in Phase 4). + + +@njit(cache=True) +def _movement_edge_time(cr, cc, nr, nc, dr, dc, + elevation, cost_mult, trail_grid, trail_friction_lookup, + barrier_grid, boundary_mode_id, + cell_size_lat_m, cell_size_lon_m, + max_grade, speed_function_id, base_speed_kmh): + """Per-edge time (s) for one 8-neighbour grid step in a SINGLE mode, or INF if + the edge is impassable / should be skipped. This is exactly the per-edge math + inlined in astar_multigoal (smooth slope penalty, trail-takes-both, barrier / + boundary rule), factored out for reuse by astar_multigoal_multimode. + astar_multigoal itself keeps its own inlined copy and is left unchanged.""" + elev_cur = elevation[cr, cc] + elev_n = elevation[nr, nc] + if math.isnan(elev_cur) or math.isnan(elev_n): + return INF + 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 + overshoot = abs(signed_grade) - max_grade + slope_penalty = 1.0 + if overshoot > 0.0: + slope_penalty = math.exp(overshoot * SLOPE_PENALTY_SCALE) + if slope_penalty > SLOPE_PENALTY_CAP: + return INF + spd = _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade) + if spd <= 1e-9: + return INF + base_time = dist * 3.6 / spd + base_time *= slope_penalty + + 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): + return INF # 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)): + return INF # 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: + return INF + 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 + return edge + + +@njit(cache=True) +def astar_multigoal_multimode( + cost_mult_stack, # 3D float64 [rows, cols, n_modes]: per-mode context mult (inf=impassable) + elevation, # 2D float64: metres (NaN = impassable) + cell_size_lat_m, # float + cell_size_lon_m, # float + max_grade_arr, # 1D float64 [n_modes]: tan(max_slope) per mode + speed_function_ids, # 1D int [n_modes]: 0=tobler 1=herzog 2=linear + base_speed_kmh_arr, # 1D float64 [n_modes] + trail_grid, # 2D uint8: 0=none else trail value (5/15/25) + trail_friction_stack, # 2D float64 [n_modes, 256]: friction by trail value per mode + barrier_grid, # 2D uint8: 255=barrier + boundary_mode_id, # int: 0=strict 1=pragmatic 2=emergency + origin_row, origin_col, + origin_modes, # 1D int: allowed start modes (seeds) + goal_rows, goal_cols, # 1D int arrays + goal_modes, # 1D int: allowed end modes + trans_rows, # 1D int [n_trans]: transition cell rows + trans_cols, # 1D int [n_trans]: transition cell cols + trans_from_mode, # 1D int [n_trans]: source mode index + trans_to_mode, # 1D int [n_trans]: target mode index + trans_cost_s, # 1D float64 [n_trans]: transition penalty seconds + disable_heuristic=False, # tests only: h≡0 turns the search into Dijkstra (admissibility oracle) +): + """A* over (row, col, mode). The first (goal cell, allowed goal mode) state + popped wins; optimal under the per-mode admissible heuristic (§10). Returns + (best_goal_idx, path, total_cost) where path is int64 (N,3) of (row,col,mode) + from an origin-mode seed to the goal. (-1, empty, inf) if unreachable.""" + rows = elevation.shape[0] + cols = elevation.shape[1] + n_modes = cost_mult_stack.shape[2] + rc = rows * cols # cells per mode-plane; heap id = mode*rc + row*cols + col (§11) + + 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 + goal_mode_ok = np.zeros(n_modes, dtype=np.bool_) + for gi in range(goal_modes.shape[0]): + goal_mode_ok[goal_modes[gi]] = True + + # §10: divide straight-line distance by the FASTEST base speed over goal modes + # -> smallest possible finishing time -> admissible lower bound. Independent of + # the state's current mode (a slow-mode state may switch to a fast mode later). + max_goal_speed = 0.0 + for gi in range(goal_modes.shape[0]): + s = base_speed_kmh_arr[goal_modes[gi]] + if s > max_goal_speed: + max_goal_speed = s + + 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 + closed = np.zeros((rows, cols, n_modes), dtype=np.bool_) + + # Per-cell transition index, built once before the loop. Sort transitions by + # packed cell key so each cell's edges are contiguous, then trans_head/trans_cnt + # give O(1) lookup -- a CSR layout, no numba-typed dict (compiles in nopython). + n_trans = trans_rows.shape[0] + trans_head = np.full((rows, cols), -1, dtype=np.int64) + trans_cnt = np.zeros((rows, cols), dtype=np.int64) + s_rows = trans_rows + s_cols = trans_cols + s_from = trans_from_mode + s_to = trans_to_mode + s_cost = trans_cost_s + if n_trans > 0: + key = np.empty(n_trans, dtype=np.int64) + for t in range(n_trans): + key[t] = trans_rows[t] * cols + trans_cols[t] + order = np.argsort(key) + s_rows = trans_rows[order] + s_cols = trans_cols[order] + s_from = trans_from_mode[order] + s_to = trans_to_mode[order] + s_cost = trans_cost_s[order] + for t in range(n_trans): + r = s_rows[t] + c = s_cols[t] + if trans_head[r, c] == -1: + trans_head[r, c] = t + trans_cnt[r, c] += 1 + + # Binary min-heap (lazy deletion). Capacity covers re-pushes from movement + + # transition relaxations across the n_modes-fold state space. + cap = rc * n_modes * 8 + 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): + if disable_heuristic: + return 0.0 + 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 / max_goal_speed # metres -> seconds at fastest goal speed + + # Seed every allowed origin mode at the origin cell. + for oi in range(origin_modes.shape[0]): + m0 = origin_modes[oi] + g_score[origin_row, origin_col, m0] = 0.0 + heap_id[hsize] = m0 * rc + origin_row * cols + origin_col + heap_f[hsize] = 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 + r2 = 2 * i + 2 + sm = i + if l < hsize and heap_f[l] < heap_f[sm]: + sm = l + if r2 < hsize and heap_f[r2] < heap_f[sm]: + sm = r2 + 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 + + m = cur_id // rc + rem = cur_id % rc + cr = rem // cols + cc = rem % cols + if closed[cr, cc, m]: + continue # stale heap entry + closed[cr, cc, m] = True + + if goal_index[cr, cc] >= 0 and goal_mode_ok[m]: + # First (goal cell, allowed goal mode) popped is optimal -- trace back. + length = 1 + node = cur_id + pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols + while parent[pr, pc, pm] != -1: + length += 1 + node = parent[pr, pc, pm] + pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols + path = np.empty((length, 3), dtype=np.int64) + node = cur_id + k = length - 1 + while node != -1: + pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols + path[k, 0] = pr + path[k, 1] = pc + path[k, 2] = pm + k -= 1 + node = parent[pr, pc, pm] + return goal_index[cr, cc], path, g_score[cr, cc, m] + + g_cur = g_score[cr, cc, m] + if math.isnan(elevation[cr, cc]): + continue + + cm = cost_mult_stack[:, :, m] + tfl = trail_friction_stack[m] + mg = max_grade_arr[m] + sfid = speed_function_ids[m] + bspd = base_speed_kmh_arr[m] + + # Movement edges: 8-neighbour grid step in the SAME mode. + 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, m]: + continue + edge = _movement_edge_time( + cr, cc, nr, nc, dr, dc, + elevation, cm, trail_grid, tfl, + barrier_grid, boundary_mode_id, + cell_size_lat_m, cell_size_lon_m, mg, sfid, bspd) + if not (edge < INF): + continue + tentative = g_cur + edge + if tentative < g_score[nr, nc, m]: + g_score[nr, nc, m] = tentative + parent[nr, nc, m] = cur_id + f = tentative + heuristic(nr, nc) + if hsize < cap: + heap_id[hsize] = m * rc + 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 + + # Transition edges: same cell, mode change m -> to_m (flat penalty, no terrain). + if n_trans > 0 and trans_head[cr, cc] != -1: + base = trans_head[cr, cc] + cnt = trans_cnt[cr, cc] + for t in range(base, base + cnt): + if s_from[t] != m: + continue + to_m = s_to[t] + if closed[cr, cc, to_m]: + continue + tentative = g_cur + s_cost[t] + if tentative < g_score[cr, cc, to_m]: + g_score[cr, cc, to_m] = tentative + parent[cr, cc, to_m] = cur_id + f = tentative + heuristic(cr, cc) + if hsize < cap: + heap_id[hsize] = to_m * rc + cr * cols + cc + 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, 3), dtype=np.int64), INF diff --git a/backend/services/navi_offroute/tests/test_offroute.py b/backend/services/navi_offroute/tests/test_offroute.py index a37d106..1fb8881 100644 --- a/backend/services/navi_offroute/tests/test_offroute.py +++ b/backend/services/navi_offroute/tests/test_offroute.py @@ -989,3 +989,191 @@ def test_hybrid_consumes_parking_candidates(monkeypatch): trans = next(f for f in out["route"]["features"] if f["properties"].get("kind") == "transition") assert trans["properties"]["name"] == "BLM Trailhead Lot" + + +# ── Multi-mode A* kernel (unified-graph Phase 2; spec §2.3 / §10 / §11) ─────── +from services.navi_offroute.astar import astar_multigoal_multimode as _mm +from services.navi_offroute.cost import MODE_PROFILES as _PROFILES + +_MODE_ORDER = ["foot", "2w", "4w", "vehicle"] # spec §2.1 fixed index order +_SFID = {"tobler": 0, "herzog": 1, "linear": 2} + + +def _mode_param_arrays(): + """Per-mode 1D param arrays (foot,2w,4w,vehicle order) + trail_friction_stack + [n_modes,256], built faithfully from MODE_PROFILES.""" + n = len(_MODE_ORDER) + max_grade = _np.empty(n, dtype=_np.float64) + sfid = _np.empty(n, dtype=_np.int64) + base = _np.empty(n, dtype=_np.float64) + tfs = _np.full((n, 256), _np.inf, dtype=_np.float64) + for mi, name in enumerate(_MODE_ORDER): + p = _PROFILES[name] + max_grade[mi] = float(_np.tan(_np.radians(p.max_slope_deg))) + sfid[mi] = _SFID[p.speed_function] + base[mi] = p.base_speed_kmh + for tv, fr in p.trail_friction.items(): + tfs[mi, tv] = _np.inf if fr is None else float(fr) + return max_grade, sfid, base, tfs + + +def _empty_trans(): + z = _np.empty(0, dtype=_np.int64) + return z, z.copy(), z.copy(), z.copy(), _np.empty(0, dtype=_np.float64) + + +def test_multimode_foot_only_parity(): + # foot-only, no transitions: the multimode kernel (1-mode stack) must reproduce + # astar_multigoal exactly -- it is a strict superset. + n = 8 + elev, mult, trail, lookup, barr = _flat_inputs(n) + foot_mg = float(_np.tan(_np.radians(_PROFILES["foot"].max_slope_deg))) + gr = _np.array([n - 1], dtype=_np.int64) + gc = _np.array([n - 1], dtype=_np.int64) + idx1, path1, cost1 = astar_multigoal( + mult, elev, 30.0, 30.0, foot_mg, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc) + + stack = mult.reshape(n, n, 1).copy() + tr, tc, tf, tt, tcost = _empty_trans() + idx2, path2, cost2 = _mm( + stack, elev, 30.0, 30.0, + _np.array([foot_mg]), _np.array([0], dtype=_np.int64), _np.array([6.0]), + trail, lookup.reshape(1, 256).copy(), barr, 2, + 0, 0, _np.array([0], dtype=_np.int64), gr, gc, _np.array([0], dtype=_np.int64), + tr, tc, tf, tt, tcost) + + assert idx2 == idx1 == 0 + assert cost2 == pytest.approx(cost1, rel=1e-9, abs=1e-9) + assert _np.array_equal(path2[:, :2], path1) # same (row,col) sequence + assert _np.all(path2[:, 2] == 0) # all foot + + +def test_multimode_parking_switch(): + # Forest corridor (foot-only) -> parking cell -> open field where vehicle is + # fast and foot is slow. The optimizer must switch foot->vehicle at the parking + # cell and beat foot-only. The cost advantage is TERRAIN-driven (no trails / no + # friction<1), which keeps the §10 heuristic admissible -- a road's <1 friction + # would make effective speed exceed base speed and break the heuristic (a known + # property of the inherited single-mode kernel too). + rows, cols, road_start = 3, 50, 25 + elev = _np.zeros((rows, cols), dtype=_np.float64) + n_modes = 4 + stack = _np.full((rows, cols, n_modes), _np.inf, dtype=_np.float64) + stack[:, :, 0] = 1.0 # foot: passable everywhere off-trail + stack[:, road_start:cols, 3] = 1.0 # vehicle: drivable only in the open field + trail = _np.zeros((rows, cols), dtype=_np.uint8) # no trails anywhere + max_grade, sfid, base, tfs = _mode_param_arrays() + barr = _np.zeros((rows, cols), dtype=_np.uint8) + + gr = _np.array([1], dtype=_np.int64) + gc = _np.array([cols - 1], dtype=_np.int64) + pr, pc = 1, road_start # parking cell, foot<->vehicle, 60 s each way + tr = _np.array([pr, pr], dtype=_np.int64) + tc = _np.array([pc, pc], dtype=_np.int64) + tf = _np.array([0, 3], dtype=_np.int64) + tt = _np.array([3, 0], dtype=_np.int64) + tcost = _np.array([60.0, 60.0], dtype=_np.float64) + om = _np.array([0], dtype=_np.int64) # start on foot + gm = _np.array([3, 0], dtype=_np.int64) # finish vehicle or foot + + idx, path, cost = _mm( + stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 1, + 1, 0, om, gr, gc, gm, tr, tc, tf, tt, tcost) + + assert idx == 0 + modes = path[:, 2] + assert modes[0] == 0 and modes[-1] == 3 # foot start, vehicle finish + switches = [k for k in range(1, len(path)) if modes[k] != modes[k - 1]] + assert len(switches) == 1 # exactly one mode change + sk = switches[0] + assert modes[sk - 1] == 0 and modes[sk] == 3 # foot -> vehicle + assert tuple(path[sk, :2]) == (pr, pc) # at the parking cell + assert tuple(path[sk - 1, :2]) == (pr, pc) # same cell, mode-change edge + + tr0, tc0, tf0, tt0, tcost0 = _empty_trans() + _, _, cost_foot_only = _mm( + stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 1, + 1, 0, _np.array([0], dtype=_np.int64), gr, gc, _np.array([0], dtype=_np.int64), + tr0, tc0, tf0, tt0, tcost0) + assert cost < cost_foot_only + + +def test_multimode_no_transitions_independent(): + # No transitions: the 4-mode search degrades to 4 independent single-mode + # searches -- the winning path never changes mode, and its cost equals the + # min over the four single-mode runs (vehicle wins on flat passable terrain). + # Off-trail only (no friction<1) keeps the heuristic admissible. + n = 12 + elev = _np.zeros((n, n), dtype=_np.float64) + n_modes = 4 + stack = _np.ones((n, n, n_modes), dtype=_np.float64) # all modes passable off-trail + trail = _np.zeros((n, n), dtype=_np.uint8) # no trails + max_grade, sfid, base, tfs = _mode_param_arrays() + barr = _np.zeros((n, n), dtype=_np.uint8) + gr = _np.array([n - 1], dtype=_np.int64) + gc = _np.array([n - 1], dtype=_np.int64) + all_modes = _np.array([0, 1, 2, 3], dtype=_np.int64) + tr, tc, tf, tt, tcost = _empty_trans() + + idx, path, cost = _mm( + stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 2, + 0, 0, all_modes, gr, gc, all_modes, tr, tc, tf, tt, tcost) + + assert _np.all(path[:, 2] == path[0, 2]) # single mode the whole way + winning_mode = int(path[0, 2]) + + single_costs = [] + for mi in range(4): + _, _, c1 = astar_multigoal( + stack[:, :, mi].copy(), elev, 30.0, 30.0, + float(max_grade[mi]), int(sfid[mi]), float(base[mi]), + trail, tfs[mi].copy(), barr, 2, 0, 0, gr, gc) + single_costs.append(c1) + assert cost == pytest.approx(min(single_costs), rel=1e-9, abs=1e-9) + assert winning_mode == int(_np.argmin(single_costs)) # vehicle (fastest base) + + +def test_multimode_heuristic_admissibility(): + # §10 admissibility: h(r,c,m) must never exceed the true optimal remaining cost. + # vehicle (global-fastest base) is an allowed goal mode, so max_goal_speed is the + # global max -> h is a true lower bound. No trails (friction<1 would let a road + # beat base speed), so effective speed <= base speed everywhere. + rows, cols = 6, 10 + rng = _np.random.RandomState(0) + elev = (rng.rand(rows, cols) * 20.0).astype(_np.float64) + n_modes = 4 + stack = _np.ones((rows, cols, n_modes), dtype=_np.float64) + stack[2:4, 3:6, 1] = _np.inf # a forest block impassable to wheeled modes + stack[2:4, 3:6, 2] = _np.inf + stack[2:4, 3:6, 3] = _np.inf + trail = _np.zeros((rows, cols), dtype=_np.uint8) + max_grade, sfid, base, tfs = _mode_param_arrays() + barr = _np.zeros((rows, cols), dtype=_np.uint8) + gr = _np.array([rows - 1], dtype=_np.int64) + gc = _np.array([cols - 1], dtype=_np.int64) + gm = _np.array([0, 3], dtype=_np.int64) # foot or vehicle finish + tr = _np.array([0, 0], dtype=_np.int64) # one foot<->vehicle transition + tc = _np.array([5, 5], dtype=_np.int64) + tf = _np.array([0, 3], dtype=_np.int64) + tt = _np.array([3, 0], dtype=_np.int64) + tcost = _np.array([60.0, 60.0], dtype=_np.float64) + + max_goal_speed = max(float(base[g]) for g in gm) + + sampled = 0 + for r in range(0, rows, 2): + for c in range(0, cols, 3): + for m in (0, 3): + d = float(_np.hypot((r - (rows - 1)) * 30.0, (c - (cols - 1)) * 30.0)) + h = d * 3.6 / max_goal_speed + # Oracle: same kernel with the heuristic disabled (Dijkstra), seeded + # only from this state -> exact true remaining cost. + _, _, true_cost = _mm( + stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 2, + r, c, _np.array([m], dtype=_np.int64), gr, gc, gm, + tr, tc, tf, tt, tcost, True) + if not _np.isfinite(true_cost): + continue + assert h <= true_cost + 1e-6 + sampled += 1 + assert sampled > 0 # the sweep actually exercised reachable states