From f2a0f81580a82bcb33bf5b72420d208f9282ba63 Mon Sep 17 00:00:00 2001 From: Matt Date: Thu, 7 May 2026 23:43:56 +0000 Subject: [PATCH 01/16] =?UTF-8?q?feat(offroute):=20Phase=20O1=20foundation?= =?UTF-8?q?=20=E2=80=94=20PMTiles=20decoder,=20Tobler=20cost,=20MCP=20path?= =?UTF-8?q?finder=20prototype?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - dem.py: Terrarium-encoded PMTiles tile reader with LRU cache - Decodes WebP tiles from planet-dem.pmtiles - Stitches tiles into numpy elevation grids for arbitrary bboxes - Provides pixel-to-latlon coordinate conversion - cost.py: Tobler off-path hiking cost function - speed = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) km/h - Max slope cutoff: 40 degrees → impassable - Returns time-to-traverse (seconds/cell) as cost metric - prototype.py: Standalone validation on Idaho test bbox - 43km × 80km bbox (~17M cells at 14m resolution) - scikit-image MCP_Geometric Dijkstra pathfinder - Outputs GeoJSON LineString with path metadata - Validated: 61.6km path, 21.3 hours effort time Co-Authored-By: Claude Opus 4.5 --- lib/offroute/__init__.py | 1 + lib/offroute/cost.py | 94 +++++++++++++ lib/offroute/dem.py | 190 ++++++++++++++++++++++++++ lib/offroute/prototype.py | 274 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 559 insertions(+) create mode 100644 lib/offroute/__init__.py create mode 100644 lib/offroute/cost.py create mode 100644 lib/offroute/dem.py create mode 100755 lib/offroute/prototype.py diff --git a/lib/offroute/__init__.py b/lib/offroute/__init__.py new file mode 100644 index 0000000..b0536cd --- /dev/null +++ b/lib/offroute/__init__.py @@ -0,0 +1 @@ +"""OFFROUTE: Off-network effort-based routing module.""" diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py new file mode 100644 index 0000000..f460ab9 --- /dev/null +++ b/lib/offroute/cost.py @@ -0,0 +1,94 @@ +""" +Tobler off-path hiking cost function for OFFROUTE. + +Computes travel time cost based on terrain slope using Tobler's +hiking function with off-trail penalty. +""" +import math +import numpy as np +from typing import Tuple + +# Maximum passable slope in degrees +MAX_SLOPE_DEG = 40.0 + +# Tobler off-path parameters +TOBLER_BASE_SPEED = 6.0 +TOBLER_OFF_TRAIL_MULT = 0.6 + + +def tobler_speed(grade: float) -> float: + """ + Calculate hiking speed using Tobler's off-path function. + + speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) + + Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). + """ + return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) + + +def compute_cost_grid( + elevation: np.ndarray, + cell_size_m: float, + cell_size_lat_m: float = None, + cell_size_lon_m: float = None +) -> np.ndarray: + """ + Compute isotropic travel cost grid from elevation data. + + Each cell's cost represents the time (in seconds) to traverse that cell, + based on the average slope from neighboring cells. + """ + if cell_size_lat_m is None: + cell_size_lat_m = cell_size_m + if cell_size_lon_m is None: + cell_size_lon_m = cell_size_m + + rows, cols = elevation.shape + + # Compute gradients in both directions + dy = np.zeros_like(elevation) + dx = np.zeros_like(elevation) + + # Central differences for interior, forward/backward at edges + dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) + dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m + dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m + + dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) + dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m + dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m + + # Compute slope magnitude (grade = rise/run) + grade_magnitude = np.sqrt(dx**2 + dy**2) + + # Convert to slope angle in degrees + slope_deg = np.degrees(np.arctan(grade_magnitude)) + + # Compute speed for each cell using Tobler function + speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) + + # Convert speed to time cost (seconds to traverse one cell) + avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 + cost = avg_cell_size * 3.6 / speed_kmh + + # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity + cost[slope_deg > MAX_SLOPE_DEG] = np.inf + + # Handle NaN elevations (no data) + cost[np.isnan(elevation)] = np.inf + + return cost + + +if __name__ == "__main__": + print("Testing Tobler speed function:") + for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: + speed = tobler_speed(grade) + print(f" Grade {grade:+.2f}: {speed:.2f} km/h") + + print("\nTesting cost grid computation:") + elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 + cost = compute_cost_grid(elev, cell_size_m=30.0) + print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") + print(f" Cost range: {cost[~np.isinf(cost)].min():.1f} - {cost[~np.isinf(cost)].max():.1f} s") diff --git a/lib/offroute/dem.py b/lib/offroute/dem.py new file mode 100644 index 0000000..f715611 --- /dev/null +++ b/lib/offroute/dem.py @@ -0,0 +1,190 @@ +""" +DEM tile reader for OFFROUTE. + +Reads elevation tiles from planet-dem.pmtiles (Terrarium-encoded WebP), +decodes them into numpy arrays, and provides a stitched elevation grid +for a given bounding box. +""" +import math +from functools import lru_cache +from io import BytesIO +from pathlib import Path +from typing import Tuple, Optional + +import numpy as np +from PIL import Image +from pmtiles.reader import MmapSource, Reader as PMTilesReader + +# Default path to the planet DEM PMTiles file +DEFAULT_DEM_PATH = Path("/mnt/nas/nav/planet-dem.pmtiles") + +# Tile size in pixels (z12 tiles are 512x512 in this tileset) +TILE_SIZE = 512 + +# Zoom level to use for elevation data +ZOOM_LEVEL = 12 + + +def terrarium_decode(rgb_array: np.ndarray) -> np.ndarray: + """ + Decode Terrarium-encoded RGB values to elevation in meters. + + Formula: elevation = (R * 256 + G + B/256) - 32768 + """ + r = rgb_array[:, :, 0].astype(np.float32) + g = rgb_array[:, :, 1].astype(np.float32) + b = rgb_array[:, :, 2].astype(np.float32) + + elevation = (r * 256.0 + g + b / 256.0) - 32768.0 + return elevation + + +def lat_lon_to_tile(lat: float, lon: float, zoom: int) -> Tuple[int, int]: + """Convert lat/lon to tile coordinates at given zoom level.""" + n = 2 ** zoom + x = int((lon + 180.0) / 360.0 * n) + lat_rad = math.radians(lat) + y = int((1.0 - math.asinh(math.tan(lat_rad)) / math.pi) / 2.0 * n) + return x, y + + +def tile_to_lat_lon(x: int, y: int, zoom: int) -> Tuple[float, float, float, float]: + """Convert tile coordinates to bounding box (north, south, west, east).""" + n = 2 ** zoom + lon_west = x / n * 360.0 - 180.0 + lon_east = (x + 1) / n * 360.0 - 180.0 + lat_north = math.degrees(math.atan(math.sinh(math.pi * (1 - 2 * y / n)))) + lat_south = math.degrees(math.atan(math.sinh(math.pi * (1 - 2 * (y + 1) / n)))) + return lat_north, lat_south, lon_west, lon_east + + +class DEMReader: + """Reader for Terrarium-encoded DEM tiles from PMTiles.""" + + def __init__(self, pmtiles_path: Path = DEFAULT_DEM_PATH, tile_cache_size: int = 128): + self.pmtiles_path = pmtiles_path + self._source = MmapSource(open(pmtiles_path, "rb")) + self._reader = PMTilesReader(self._source) + self._header = self._reader.header() + self._decode_tile = lru_cache(maxsize=tile_cache_size)(self._decode_tile_impl) + + def _decode_tile_impl(self, z: int, x: int, y: int) -> Optional[np.ndarray]: + """Fetch and decode a single tile.""" + tile_data = self._reader.get(z, x, y) + if tile_data is None: + return None + + img = Image.open(BytesIO(tile_data)) + rgb_array = np.array(img) + + if rgb_array.shape[2] == 4: + rgb_array = rgb_array[:, :, :3] + + elevation = terrarium_decode(rgb_array) + return elevation + + def get_elevation_grid( + self, + south: float, + north: float, + west: float, + east: float, + zoom: int = ZOOM_LEVEL + ) -> Tuple[np.ndarray, dict]: + """Get a stitched elevation grid for the given bounding box.""" + x_min, y_max = lat_lon_to_tile(south, west, zoom) + x_max, y_min = lat_lon_to_tile(north, east, zoom) + + n = 2 ** zoom + x_min = max(0, x_min) + x_max = min(n - 1, x_max) + y_min = max(0, y_min) + y_max = min(n - 1, y_max) + + n_tiles_x = x_max - x_min + 1 + n_tiles_y = y_max - y_min + 1 + out_height = n_tiles_y * TILE_SIZE + out_width = n_tiles_x * TILE_SIZE + + elevation = np.full((out_height, out_width), np.nan, dtype=np.float32) + + for ty in range(y_min, y_max + 1): + for tx in range(x_min, x_max + 1): + tile_elev = self._decode_tile(zoom, tx, ty) + if tile_elev is not None: + out_y = (ty - y_min) * TILE_SIZE + out_x = (tx - x_min) * TILE_SIZE + elevation[out_y:out_y + TILE_SIZE, out_x:out_x + TILE_SIZE] = tile_elev + + grid_north, _, grid_west, _ = tile_to_lat_lon(x_min, y_min, zoom) + _, grid_south, _, grid_east = tile_to_lat_lon(x_max, y_max, zoom) + + pixel_size_lat = (grid_north - grid_south) / out_height + pixel_size_lon = (grid_east - grid_west) / out_width + + origin_lat = grid_north - pixel_size_lat / 2 + origin_lon = grid_west + pixel_size_lon / 2 + + center_lat = (south + north) / 2 + lat_m = 111320.0 + lon_m = 111320.0 * math.cos(math.radians(center_lat)) + cell_size_lat_m = abs(pixel_size_lat) * lat_m + cell_size_lon_m = abs(pixel_size_lon) * lon_m + cell_size_m = (cell_size_lat_m + cell_size_lon_m) / 2 + + row_start = int((grid_north - north) / abs(pixel_size_lat)) + row_end = int((grid_north - south) / abs(pixel_size_lat)) + col_start = int((west - grid_west) / pixel_size_lon) + col_end = int((east - grid_west) / pixel_size_lon) + + row_start = max(0, row_start) + row_end = min(out_height, row_end) + col_start = max(0, col_start) + col_end = min(out_width, col_end) + + elevation = elevation[row_start:row_end, col_start:col_end] + + origin_lat = grid_north - (row_start + 0.5) * abs(pixel_size_lat) + origin_lon = grid_west + (col_start + 0.5) * pixel_size_lon + + metadata = { + "bounds": (south, north, west, east), + "pixel_size_lat": -abs(pixel_size_lat), + "pixel_size_lon": pixel_size_lon, + "origin_lat": origin_lat, + "origin_lon": origin_lon, + "cell_size_m": cell_size_m, + "shape": elevation.shape, + } + + return elevation, metadata + + def pixel_to_latlon(self, row: int, col: int, metadata: dict) -> Tuple[float, float]: + """Convert pixel coordinates to lat/lon.""" + lat = metadata["origin_lat"] + row * metadata["pixel_size_lat"] + lon = metadata["origin_lon"] + col * metadata["pixel_size_lon"] + return lat, lon + + def latlon_to_pixel(self, lat: float, lon: float, metadata: dict) -> Tuple[int, int]: + """Convert lat/lon to pixel coordinates.""" + row = int((metadata["origin_lat"] - lat) / abs(metadata["pixel_size_lat"])) + col = int((lon - metadata["origin_lon"]) / metadata["pixel_size_lon"]) + return row, col + + def close(self): + """Close the PMTiles file.""" + pass # MmapSource handles cleanup + + +if __name__ == "__main__": + reader = DEMReader() + elevation, meta = reader.get_elevation_grid( + south=42.4, north=42.6, west=-114.5, east=-114.3 + ) + print(f"Elevation grid shape: {elevation.shape}") + print(f"Cell size: {meta['cell_size_m']:.1f} m") + print(f"Elevation range: {np.nanmin(elevation):.1f} - {np.nanmax(elevation):.1f} m") + center_row, center_col = elevation.shape[0] // 2, elevation.shape[1] // 2 + lat, lon = reader.pixel_to_latlon(center_row, center_col, meta) + print(f"Center pixel lat/lon: {lat:.4f}, {lon:.4f}") + reader.close() diff --git a/lib/offroute/prototype.py b/lib/offroute/prototype.py new file mode 100755 index 0000000..0790a32 --- /dev/null +++ b/lib/offroute/prototype.py @@ -0,0 +1,274 @@ +#!/usr/bin/env python3 +""" +OFFROUTE Phase O1 Prototype + +Validates the PMTiles decoder, Tobler cost function, and MCP pathfinder +on a real Idaho bounding box. + +Test bbox (four Idaho towns as corners): + SW: Rogerson, ID (~42.21, -114.60) + NW: Buhl, ID (~42.60, -114.76) + NE: Burley, ID (~42.54, -113.79) + SE: Oakley, ID (~42.24, -113.88) + Approximate bbox: south=42.21, north=42.60, west=-114.76, east=-113.79 +""" +import json +import time +import sys +from pathlib import Path + +import numpy as np +from skimage.graph import MCP_Geometric + +# Add parent to path for imports +sys.path.insert(0, str(Path(__file__).parent.parent.parent)) + +from lib.offroute.dem import DEMReader +from lib.offroute.cost import compute_cost_grid + +# Test bounding box +BBOX = { + "south": 42.21, + "north": 42.60, + "west": -114.76, + "east": -113.79, +} + +# Start point: wilderness area south of Twin Falls +# (in the Sawtooth National Forest foothills) +START_LAT = 42.35 +START_LON = -114.50 + +# End point: near Burley, ID (on road network) +END_LAT = 42.52 +END_LON = -113.85 + +# Output file +OUTPUT_PATH = Path("/opt/recon/data/offroute-test.geojson") + +# Memory limit in GB +MEMORY_LIMIT_GB = 12 + + +def check_memory_usage(): + """Check current memory usage and abort if over limit.""" + try: + import psutil + process = psutil.Process() + mem_gb = process.memory_info().rss / (1024**3) + if mem_gb > MEMORY_LIMIT_GB: + print(f"ERROR: Memory usage {mem_gb:.1f}GB exceeds {MEMORY_LIMIT_GB}GB limit") + sys.exit(1) + return mem_gb + except ImportError: + return 0 + + +def main(): + print("=" * 60) + print("OFFROUTE Phase O1 Prototype") + print("=" * 60) + + t0 = time.time() + + # Step 1: Load elevation data + print(f"\n[1] Loading DEM for bbox: {BBOX}") + reader = DEMReader() + + t1 = time.time() + elevation, meta = reader.get_elevation_grid( + south=BBOX["south"], + north=BBOX["north"], + west=BBOX["west"], + east=BBOX["east"], + ) + t2 = time.time() + + print(f" Elevation grid shape: {elevation.shape}") + print(f" Cell count: {elevation.size:,}") + print(f" Cell size: {meta['cell_size_m']:.1f} m") + print(f" Elevation range: {np.nanmin(elevation):.0f} - {np.nanmax(elevation):.0f} m") + print(f" Load time: {t2 - t1:.1f}s") + + mem = check_memory_usage() + if mem > 0: + print(f" Memory usage: {mem:.1f} GB") + + # Step 2: Compute cost grid + print(f"\n[2] Computing Tobler cost grid...") + t3 = time.time() + cost = compute_cost_grid(elevation, cell_size_m=meta["cell_size_m"]) + t4 = time.time() + + finite_cost = cost[~np.isinf(cost)] + print(f" Cost range: {finite_cost.min():.1f} - {finite_cost.max():.1f} s/cell") + print(f" Impassable cells: {np.sum(np.isinf(cost)):,} ({100*np.sum(np.isinf(cost))/cost.size:.1f}%)") + print(f" Compute time: {t4 - t3:.1f}s") + + mem = check_memory_usage() + if mem > 0: + print(f" Memory usage: {mem:.1f} GB") + + # Step 3: Convert start/end to pixel coordinates + print(f"\n[3] Converting coordinates...") + start_row, start_col = reader.latlon_to_pixel(START_LAT, START_LON, meta) + end_row, end_col = reader.latlon_to_pixel(END_LAT, END_LON, meta) + + print(f" Start: ({START_LAT}, {START_LON}) -> pixel ({start_row}, {start_col})") + print(f" End: ({END_LAT}, {END_LON}) -> pixel ({end_row}, {end_col})") + + # Validate coordinates are within bounds + rows, cols = elevation.shape + if not (0 <= start_row < rows and 0 <= start_col < cols): + print(f"ERROR: Start point outside grid bounds") + sys.exit(1) + if not (0 <= end_row < rows and 0 <= end_col < cols): + print(f"ERROR: End point outside grid bounds") + sys.exit(1) + + start_elev = elevation[start_row, start_col] + end_elev = elevation[end_row, end_col] + print(f" Start elevation: {start_elev:.0f} m") + print(f" End elevation: {end_elev:.0f} m") + + # Step 4: Run MCP pathfinder + print(f"\n[4] Running MCP_Geometric pathfinder...") + t5 = time.time() + + # MCP_Geometric finds minimum cost path + # It uses Dijkstra's algorithm internally + mcp = MCP_Geometric(cost, fully_connected=True) + + # Find costs from start to all reachable cells + cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) + t6 = time.time() + + print(f" Dijkstra completed in {t6 - t5:.1f}s") + + # Get cost to reach end point + end_cost = cumulative_costs[end_row, end_col] + print(f" Total cost to endpoint: {end_cost:.0f} seconds ({end_cost/60:.1f} minutes)") + + if np.isinf(end_cost): + print("ERROR: No path found to endpoint (blocked by impassable terrain)") + sys.exit(1) + + # Trace back the path + t7 = time.time() + path_indices = mcp.traceback((end_row, end_col)) + t8 = time.time() + + print(f" Traceback completed in {t8 - t7:.2f}s") + print(f" Path length: {len(path_indices)} cells") + + mem = check_memory_usage() + if mem > 0: + print(f" Memory usage: {mem:.1f} GB") + + # Step 5: Convert path to coordinates and compute stats + print(f"\n[5] Converting path to GeoJSON...") + + coordinates = [] + elevations = [] + + for row, col in path_indices: + lat, lon = reader.pixel_to_latlon(row, col, meta) + elev = elevation[row, col] + coordinates.append([lon, lat]) # GeoJSON is [lon, lat] + elevations.append(elev) + + # Compute path distance + total_distance_m = 0 + for i in range(1, len(coordinates)): + lon1, lat1 = coordinates[i-1] + lon2, lat2 = coordinates[i] + # Haversine formula + R = 6371000 # Earth radius in meters + dlat = np.radians(lat2 - lat1) + dlon = np.radians(lon2 - lon1) + a = np.sin(dlat/2)**2 + np.cos(np.radians(lat1)) * np.cos(np.radians(lat2)) * np.sin(dlon/2)**2 + c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1-a)) + total_distance_m += R * c + + # Compute elevation gain/loss + elev_arr = np.array(elevations) + elev_diff = np.diff(elev_arr) + elev_gain = np.sum(elev_diff[elev_diff > 0]) + elev_loss = np.sum(np.abs(elev_diff[elev_diff < 0])) + + # Build GeoJSON + geojson = { + "type": "Feature", + "properties": { + "type": "offroute_prototype", + "start": {"lat": START_LAT, "lon": START_LON}, + "end": {"lat": END_LAT, "lon": END_LON}, + "total_time_seconds": float(end_cost), + "total_time_minutes": float(end_cost / 60), + "total_distance_m": float(total_distance_m), + "total_distance_km": float(total_distance_m / 1000), + "elevation_gain_m": float(elev_gain), + "elevation_loss_m": float(elev_loss), + "min_elevation_m": float(np.min(elev_arr)), + "max_elevation_m": float(np.max(elev_arr)), + "cell_count": len(path_indices), + "cell_size_m": meta["cell_size_m"], + }, + "geometry": { + "type": "LineString", + "coordinates": coordinates, + } + } + + # Write output + OUTPUT_PATH.parent.mkdir(parents=True, exist_ok=True) + with open(OUTPUT_PATH, "w") as f: + json.dump(geojson, f, indent=2) + + t_end = time.time() + + # Final report + print(f"\n" + "=" * 60) + print("RESULTS") + print("=" * 60) + print(f"Start: ({START_LAT:.4f}, {START_LON:.4f})") + print(f"End: ({END_LAT:.4f}, {END_LON:.4f})") + print(f"Total effort: {end_cost/60:.1f} minutes ({end_cost/3600:.2f} hours)") + print(f"Distance: {total_distance_m/1000:.2f} km") + print(f"Elevation gain: {elev_gain:.0f} m") + print(f"Elevation loss: {elev_loss:.0f} m") + print(f"Elevation range: {np.min(elev_arr):.0f} - {np.max(elev_arr):.0f} m") + print(f"Path cells: {len(path_indices):,}") + print(f"Wall time: {t_end - t0:.1f}s") + print(f"\nOutput saved to: {OUTPUT_PATH}") + + # Validation checks + print(f"\n" + "-" * 60) + print("VALIDATION") + print("-" * 60) + + # Check coordinates are within bbox + lons = [c[0] for c in coordinates] + lats = [c[1] for c in coordinates] + lon_ok = BBOX["west"] <= min(lons) and max(lons) <= BBOX["east"] + lat_ok = BBOX["south"] <= min(lats) and max(lats) <= BBOX["north"] + print(f"Coordinates within bbox: {'PASS' if lon_ok and lat_ok else 'FAIL'}") + + # Check path is not trivial + is_nontrivial = len(path_indices) > 10 and total_distance_m > 1000 + print(f"Path is non-trivial: {'PASS' if is_nontrivial else 'FAIL'}") + + # Check it's not a straight line (measure sinuosity) + straight_line_dist = np.sqrt( + (coordinates[-1][0] - coordinates[0][0])**2 + + (coordinates[-1][1] - coordinates[0][1])**2 + ) * 111000 # rough degrees to meters + sinuosity = total_distance_m / max(straight_line_dist, 1) + print(f"Sinuosity: {sinuosity:.2f} (>1.0 means path curves around obstacles)") + + reader.close() + print("\nPrototype completed successfully.") + + +if __name__ == "__main__": + main() From 26d4bc74784c7586ce14318a1d2cbf8d00f13b8e Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 06:33:45 +0000 Subject: [PATCH 02/16] =?UTF-8?q?feat(offroute):=20Phase=20O2b=20=E2=80=94?= =?UTF-8?q?=20WorldCover=20friction=20integration,=20lake=20avoidance=20va?= =?UTF-8?q?lidated?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - New friction.py: reads WorldCover friction VRT, resamples to match elevation grid, provides point sampling for validation - Modified cost.py: accepts optional friction array, multiplies Tobler time cost by friction multiplier, inf for water/nodata (255/0) - Modified prototype.py: loads friction layer, passes to cost function, validates path avoids water cells (friction=255) Validated on Idaho test bbox: - Path avoids Murtaugh Lake (no water cells on path) - Friction along path: min=10, max=20, mean=10.2 - Effort increased 3.4% vs Phase O1 due to friction multipliers Co-Authored-By: Claude Opus 4.5 --- lib/offroute/cost.py | 226 ++++++++++++++++++++++---------------- lib/offroute/friction.py | 137 +++++++++++++++++++++++ lib/offroute/prototype.py | 190 +++++++++++++++++++++++++------- 3 files changed, 420 insertions(+), 133 deletions(-) create mode 100644 lib/offroute/friction.py diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py index f460ab9..3607de6 100644 --- a/lib/offroute/cost.py +++ b/lib/offroute/cost.py @@ -1,94 +1,132 @@ -""" -Tobler off-path hiking cost function for OFFROUTE. - -Computes travel time cost based on terrain slope using Tobler's -hiking function with off-trail penalty. -""" -import math -import numpy as np -from typing import Tuple - -# Maximum passable slope in degrees -MAX_SLOPE_DEG = 40.0 - -# Tobler off-path parameters -TOBLER_BASE_SPEED = 6.0 -TOBLER_OFF_TRAIL_MULT = 0.6 - - -def tobler_speed(grade: float) -> float: - """ - Calculate hiking speed using Tobler's off-path function. - - speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) - - Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). - """ - return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) - - -def compute_cost_grid( - elevation: np.ndarray, - cell_size_m: float, - cell_size_lat_m: float = None, - cell_size_lon_m: float = None -) -> np.ndarray: - """ - Compute isotropic travel cost grid from elevation data. - - Each cell's cost represents the time (in seconds) to traverse that cell, - based on the average slope from neighboring cells. - """ - if cell_size_lat_m is None: - cell_size_lat_m = cell_size_m - if cell_size_lon_m is None: - cell_size_lon_m = cell_size_m - - rows, cols = elevation.shape - - # Compute gradients in both directions - dy = np.zeros_like(elevation) - dx = np.zeros_like(elevation) - - # Central differences for interior, forward/backward at edges - dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) - dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m - dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m - - dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) - dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m - dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m - - # Compute slope magnitude (grade = rise/run) - grade_magnitude = np.sqrt(dx**2 + dy**2) - - # Convert to slope angle in degrees - slope_deg = np.degrees(np.arctan(grade_magnitude)) - - # Compute speed for each cell using Tobler function - speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) - - # Convert speed to time cost (seconds to traverse one cell) - avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 - cost = avg_cell_size * 3.6 / speed_kmh - - # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity - cost[slope_deg > MAX_SLOPE_DEG] = np.inf - - # Handle NaN elevations (no data) - cost[np.isnan(elevation)] = np.inf - - return cost - - -if __name__ == "__main__": - print("Testing Tobler speed function:") - for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: - speed = tobler_speed(grade) - print(f" Grade {grade:+.2f}: {speed:.2f} km/h") - - print("\nTesting cost grid computation:") - elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 - cost = compute_cost_grid(elev, cell_size_m=30.0) - print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") - print(f" Cost range: {cost[~np.isinf(cost)].min():.1f} - {cost[~np.isinf(cost)].max():.1f} s") +""" +Tobler off-path hiking cost function for OFFROUTE. + +Computes travel time cost based on terrain slope using Tobler's +hiking function with off-trail penalty. Optionally applies friction +multipliers from land cover data. +""" +import math +import numpy as np +from typing import Optional + +# Maximum passable slope in degrees +MAX_SLOPE_DEG = 40.0 + +# Tobler off-path parameters +TOBLER_BASE_SPEED = 6.0 +TOBLER_OFF_TRAIL_MULT = 0.6 + + +def tobler_speed(grade: float) -> float: + """ + Calculate hiking speed using Tobler's off-path function. + + speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) + + Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). + """ + return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) + + +def compute_cost_grid( + elevation: np.ndarray, + cell_size_m: float, + cell_size_lat_m: float = None, + cell_size_lon_m: float = None, + friction: Optional[np.ndarray] = None +) -> np.ndarray: + """ + Compute isotropic travel cost grid from elevation data. + + Each cell's cost represents the time (in seconds) to traverse that cell, + based on the average slope from neighboring cells. + + Args: + elevation: 2D array of elevation values in meters + cell_size_m: Average cell size in meters + cell_size_lat_m: Cell size in latitude direction (optional) + cell_size_lon_m: Cell size in longitude direction (optional) + friction: Optional 2D array of friction multipliers. + Values should be float (1.0 = baseline, 2.0 = 2x slower). + np.inf marks impassable cells. + If None, no friction is applied (backward compatible). + + Returns: + 2D array of travel cost in seconds per cell. + np.inf for impassable cells. + """ + if cell_size_lat_m is None: + cell_size_lat_m = cell_size_m + if cell_size_lon_m is None: + cell_size_lon_m = cell_size_m + + rows, cols = elevation.shape + + # Compute gradients in both directions + dy = np.zeros_like(elevation) + dx = np.zeros_like(elevation) + + # Central differences for interior, forward/backward at edges + dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) + dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m + dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m + + dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) + dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m + dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m + + # Compute slope magnitude (grade = rise/run) + grade_magnitude = np.sqrt(dx**2 + dy**2) + + # Convert to slope angle in degrees + slope_deg = np.degrees(np.arctan(grade_magnitude)) + + # Compute speed for each cell using Tobler function + speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) + + # Convert speed to time cost (seconds to traverse one cell) + avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 + cost = avg_cell_size * 3.6 / speed_kmh + + # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity + cost[slope_deg > MAX_SLOPE_DEG] = np.inf + + # Handle NaN elevations (no data) + cost[np.isnan(elevation)] = np.inf + + # Apply friction multipliers if provided + if friction is not None: + if friction.shape != elevation.shape: + raise ValueError( + f"Friction shape {friction.shape} does not match elevation shape {elevation.shape}" + ) + # Multiply cost by friction (inf * anything = inf, which is correct) + cost = cost * friction + + return cost + + +if __name__ == "__main__": + print("Testing Tobler speed function:") + for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: + speed = tobler_speed(grade) + print(f" Grade {grade:+.2f}: {speed:.2f} km/h") + + print("\nTesting cost grid computation (no friction):") + elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 + cost = compute_cost_grid(elev, cell_size_m=30.0) + print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") + finite = cost[~np.isinf(cost)] + if len(finite) > 0: + print(f" Cost range: {finite.min():.1f} - {finite.max():.1f} s") + else: + print(f" All cells impassable (test data too steep)") + + print("\nTesting cost grid with friction:") + elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain + friction = np.ones((10, 10), dtype=np.float32) * 1.5 # 1.5x friction + friction[5, 5] = np.inf # one impassable cell + cost = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) + print(f" Base cost (flat, 30m cell): {30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)):.1f} s") + print(f" With 1.5x friction: {cost[0, 0]:.1f} s") + print(f" Impassable cells: {np.sum(np.isinf(cost))}") diff --git a/lib/offroute/friction.py b/lib/offroute/friction.py new file mode 100644 index 0000000..32df0c0 --- /dev/null +++ b/lib/offroute/friction.py @@ -0,0 +1,137 @@ +""" +Friction layer reader for OFFROUTE. + +Reads friction values from the WorldCover friction VRT and resamples +to match the elevation grid dimensions. +""" +import numpy as np +from pathlib import Path +from typing import Tuple, Optional + +try: + import rasterio + from rasterio.windows import from_bounds + from rasterio.enums import Resampling +except ImportError: + raise ImportError("rasterio is required for friction layer support") + +# Default path to the friction VRT +DEFAULT_FRICTION_PATH = Path("/mnt/nav/worldcover/friction/friction_conus.vrt") + + +class FrictionReader: + """Reader for WorldCover friction raster.""" + + def __init__(self, friction_path: Path = DEFAULT_FRICTION_PATH): + self.friction_path = friction_path + self._dataset = None + + def _open(self): + """Lazy open the dataset.""" + if self._dataset is None: + self._dataset = rasterio.open(self.friction_path) + return self._dataset + + def get_friction_grid( + self, + south: float, + north: float, + west: float, + east: float, + target_shape: Tuple[int, int] + ) -> np.ndarray: + """ + Get friction values for a bounding box, resampled to target shape. + + Args: + south, north, west, east: Bounding box coordinates + target_shape: (rows, cols) to resample to (matches elevation grid) + + Returns: + np.ndarray of uint8 friction values, same shape as target_shape. + Values: 10-40 = friction multiplier (divide by 10) + 255 = impassable + 0 = nodata (treat as impassable) + """ + ds = self._open() + + # Create a window from the bounding box + window = from_bounds(west, south, east, north, ds.transform) + + # Read with resampling to target shape + # Use nearest neighbor for categorical data + friction = ds.read( + 1, + window=window, + out_shape=target_shape, + resampling=Resampling.nearest + ) + + return friction + + def sample_point(self, lat: float, lon: float) -> int: + """Sample friction value at a single point.""" + ds = self._open() + + # Get pixel coordinates + row, col = ds.index(lon, lat) + + # Check bounds + if row < 0 or row >= ds.height or col < 0 or col >= ds.width: + return 0 # Out of bounds = nodata + + # Read single pixel + window = rasterio.windows.Window(col, row, 1, 1) + value = ds.read(1, window=window) + return int(value[0, 0]) + + def close(self): + """Close the dataset.""" + if self._dataset is not None: + self._dataset.close() + self._dataset = None + + +def friction_to_multiplier(friction: np.ndarray) -> np.ndarray: + """ + Convert friction values to cost multipliers. + + Args: + friction: uint8 array of friction values + + Returns: + float32 array of multipliers. + Values 10-40 become 1.0-4.0 (divide by 10). + Values 0 or 255 become np.inf (impassable). + """ + multiplier = friction.astype(np.float32) / 10.0 + + # Mark impassable cells + multiplier[friction == 0] = np.inf # nodata + multiplier[friction == 255] = np.inf # water/impassable + + return multiplier + + +if __name__ == "__main__": + print("Testing FrictionReader...") + + reader = FrictionReader() + + # Test point sampling - Murtaugh Lake (should be water = 255) + lake_lat, lake_lon = 42.47, -114.15 + lake_friction = reader.sample_point(lake_lat, lake_lon) + print(f"Murtaugh Lake ({lake_lat}, {lake_lon}): friction = {lake_friction}") + print(f" Expected: 255 (water/impassable)") + + # Test grid read for small bbox + friction = reader.get_friction_grid( + south=42.4, north=42.5, west=-114.2, east=-114.1, + target_shape=(100, 100) + ) + print(f"\nGrid test shape: {friction.shape}") + print(f"Unique values: {np.unique(friction)}") + print(f"Water cells (255): {np.sum(friction == 255)}") + + reader.close() + print("\nFrictionReader test complete.") diff --git a/lib/offroute/prototype.py b/lib/offroute/prototype.py index 0790a32..9822021 100755 --- a/lib/offroute/prototype.py +++ b/lib/offroute/prototype.py @@ -1,9 +1,11 @@ #!/usr/bin/env python3 """ -OFFROUTE Phase O1 Prototype +OFFROUTE Phase O2b Prototype -Validates the PMTiles decoder, Tobler cost function, and MCP pathfinder -on a real Idaho bounding box. +Validates the PMTiles decoder, Tobler cost function, WorldCover friction +integration, and MCP pathfinder on a real Idaho bounding box. + +Now includes friction layer to avoid water bodies like Murtaugh Lake. Test bbox (four Idaho towns as corners): SW: Rogerson, ID (~42.21, -114.60) @@ -25,6 +27,7 @@ sys.path.insert(0, str(Path(__file__).parent.parent.parent)) from lib.offroute.dem import DEMReader from lib.offroute.cost import compute_cost_grid +from lib.offroute.friction import FrictionReader, friction_to_multiplier # Test bounding box BBOX = { @@ -43,8 +46,18 @@ START_LON = -114.50 END_LAT = 42.52 END_LON = -113.85 -# Output file -OUTPUT_PATH = Path("/opt/recon/data/offroute-test.geojson") +# Murtaugh Lake - actual water extent from WorldCover +LAKE_BOUNDS = { + "south": 42.44, + "north": 42.50, + "west": -114.20, + "east": -114.10, +} +LAKE_CENTER = (42.465, -114.155) # Verified water in WorldCover + +# Output files +OUTPUT_PATH_O1 = Path("/opt/recon/data/offroute-test.geojson") +OUTPUT_PATH_FRICTION = Path("/opt/recon/data/offroute-test-friction.geojson") # Memory limit in GB MEMORY_LIMIT_GB = 12 @@ -64,19 +77,28 @@ def check_memory_usage(): return 0 +def path_crosses_lake(coordinates, lake_bounds): + """Check if any path coordinates fall within the lake bounding box.""" + for lon, lat in coordinates: + if (lake_bounds["south"] <= lat <= lake_bounds["north"] and + lake_bounds["west"] <= lon <= lake_bounds["east"]): + return True, (lat, lon) + return False, None + + def main(): print("=" * 60) - print("OFFROUTE Phase O1 Prototype") + print("OFFROUTE Phase O2b Prototype (with Friction)") print("=" * 60) t0 = time.time() # Step 1: Load elevation data print(f"\n[1] Loading DEM for bbox: {BBOX}") - reader = DEMReader() + dem_reader = DEMReader() t1 = time.time() - elevation, meta = reader.get_elevation_grid( + elevation, meta = dem_reader.get_elevation_grid( south=BBOX["south"], north=BBOX["north"], west=BBOX["west"], @@ -94,25 +116,67 @@ def main(): if mem > 0: print(f" Memory usage: {mem:.1f} GB") - # Step 2: Compute cost grid - print(f"\n[2] Computing Tobler cost grid...") + # Step 2: Load friction data + print(f"\n[2] Loading WorldCover friction layer...") + t2a = time.time() + + friction_reader = FrictionReader() + + # Validate lake is marked as impassable + lake_friction = friction_reader.sample_point(LAKE_CENTER[0], LAKE_CENTER[1]) + print(f" Murtaugh Lake center ({LAKE_CENTER[0]}, {LAKE_CENTER[1]}): friction = {lake_friction}") + if lake_friction != 255: + print(f" WARNING: Lake not marked as water (expected 255, got {lake_friction})") + else: + print(f" Lake correctly marked as impassable (255)") + + # Load friction grid matching elevation shape + friction_raw = friction_reader.get_friction_grid( + south=BBOX["south"], + north=BBOX["north"], + west=BBOX["west"], + east=BBOX["east"], + target_shape=elevation.shape + ) + t2b = time.time() + + # Convert to multipliers + friction_mult = friction_to_multiplier(friction_raw) + + impassable_count = np.sum(np.isinf(friction_mult)) + print(f" Friction grid shape: {friction_raw.shape}") + print(f" Unique friction values: {np.unique(friction_raw[friction_raw > 0])}") + print(f" Impassable cells (water/nodata): {impassable_count:,} ({100*impassable_count/friction_raw.size:.1f}%)") + print(f" Load time: {t2b - t2a:.1f}s") + + mem = check_memory_usage() + if mem > 0: + print(f" Memory usage: {mem:.1f} GB") + + # Step 3: Compute cost grid with friction + print(f"\n[3] Computing Tobler cost grid with friction...") t3 = time.time() - cost = compute_cost_grid(elevation, cell_size_m=meta["cell_size_m"]) + cost = compute_cost_grid( + elevation, + cell_size_m=meta["cell_size_m"], + friction=friction_mult + ) t4 = time.time() finite_cost = cost[~np.isinf(cost)] + total_impassable = np.sum(np.isinf(cost)) print(f" Cost range: {finite_cost.min():.1f} - {finite_cost.max():.1f} s/cell") - print(f" Impassable cells: {np.sum(np.isinf(cost)):,} ({100*np.sum(np.isinf(cost))/cost.size:.1f}%)") + print(f" Total impassable cells: {total_impassable:,} ({100*total_impassable/cost.size:.1f}%)") print(f" Compute time: {t4 - t3:.1f}s") mem = check_memory_usage() if mem > 0: print(f" Memory usage: {mem:.1f} GB") - # Step 3: Convert start/end to pixel coordinates - print(f"\n[3] Converting coordinates...") - start_row, start_col = reader.latlon_to_pixel(START_LAT, START_LON, meta) - end_row, end_col = reader.latlon_to_pixel(END_LAT, END_LON, meta) + # Step 4: Convert start/end to pixel coordinates + print(f"\n[4] Converting coordinates...") + start_row, start_col = dem_reader.latlon_to_pixel(START_LAT, START_LON, meta) + end_row, end_col = dem_reader.latlon_to_pixel(END_LAT, END_LON, meta) print(f" Start: ({START_LAT}, {START_LON}) -> pixel ({start_row}, {start_col})") print(f" End: ({END_LAT}, {END_LON}) -> pixel ({end_row}, {end_col})") @@ -131,21 +195,16 @@ def main(): print(f" Start elevation: {start_elev:.0f} m") print(f" End elevation: {end_elev:.0f} m") - # Step 4: Run MCP pathfinder - print(f"\n[4] Running MCP_Geometric pathfinder...") + # Step 5: Run MCP pathfinder + print(f"\n[5] Running MCP_Geometric pathfinder...") t5 = time.time() - # MCP_Geometric finds minimum cost path - # It uses Dijkstra's algorithm internally mcp = MCP_Geometric(cost, fully_connected=True) - - # Find costs from start to all reachable cells cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) t6 = time.time() print(f" Dijkstra completed in {t6 - t5:.1f}s") - # Get cost to reach end point end_cost = cumulative_costs[end_row, end_col] print(f" Total cost to endpoint: {end_cost:.0f} seconds ({end_cost/60:.1f} minutes)") @@ -153,7 +212,6 @@ def main(): print("ERROR: No path found to endpoint (blocked by impassable terrain)") sys.exit(1) - # Trace back the path t7 = time.time() path_indices = mcp.traceback((end_row, end_col)) t8 = time.time() @@ -165,25 +223,27 @@ def main(): if mem > 0: print(f" Memory usage: {mem:.1f} GB") - # Step 5: Convert path to coordinates and compute stats - print(f"\n[5] Converting path to GeoJSON...") + # Step 6: Convert path to coordinates and compute stats + print(f"\n[6] Converting path to GeoJSON...") coordinates = [] elevations = [] + friction_values = [] for row, col in path_indices: - lat, lon = reader.pixel_to_latlon(row, col, meta) + lat, lon = dem_reader.pixel_to_latlon(row, col, meta) elev = elevation[row, col] - coordinates.append([lon, lat]) # GeoJSON is [lon, lat] + fric = friction_raw[row, col] + coordinates.append([lon, lat]) elevations.append(elev) + friction_values.append(fric) # Compute path distance total_distance_m = 0 for i in range(1, len(coordinates)): lon1, lat1 = coordinates[i-1] lon2, lat2 = coordinates[i] - # Haversine formula - R = 6371000 # Earth radius in meters + R = 6371000 dlat = np.radians(lat2 - lat1) dlon = np.radians(lon2 - lon1) a = np.sin(dlat/2)**2 + np.cos(np.radians(lat1)) * np.cos(np.radians(lat2)) * np.sin(dlon/2)**2 @@ -196,11 +256,16 @@ def main(): elev_gain = np.sum(elev_diff[elev_diff > 0]) elev_loss = np.sum(np.abs(elev_diff[elev_diff < 0])) + # Friction stats along path + fric_arr = np.array(friction_values) + valid_fric = fric_arr[(fric_arr > 0) & (fric_arr < 255)] + # Build GeoJSON geojson = { "type": "Feature", "properties": { - "type": "offroute_prototype", + "type": "offroute_prototype_friction", + "phase": "O2b", "start": {"lat": START_LAT, "lon": START_LON}, "end": {"lat": END_LAT, "lon": END_LON}, "total_time_seconds": float(end_cost), @@ -211,6 +276,9 @@ def main(): "elevation_loss_m": float(elev_loss), "min_elevation_m": float(np.min(elev_arr)), "max_elevation_m": float(np.max(elev_arr)), + "friction_min": int(valid_fric.min()) if len(valid_fric) > 0 else 0, + "friction_max": int(valid_fric.max()) if len(valid_fric) > 0 else 0, + "friction_mean": float(valid_fric.mean()) if len(valid_fric) > 0 else 0, "cell_count": len(path_indices), "cell_size_m": meta["cell_size_m"], }, @@ -221,15 +289,15 @@ def main(): } # Write output - OUTPUT_PATH.parent.mkdir(parents=True, exist_ok=True) - with open(OUTPUT_PATH, "w") as f: + OUTPUT_PATH_FRICTION.parent.mkdir(parents=True, exist_ok=True) + with open(OUTPUT_PATH_FRICTION, "w") as f: json.dump(geojson, f, indent=2) t_end = time.time() # Final report print(f"\n" + "=" * 60) - print("RESULTS") + print("RESULTS (Phase O2b with Friction)") print("=" * 60) print(f"Start: ({START_LAT:.4f}, {START_LON:.4f})") print(f"End: ({END_LAT:.4f}, {END_LON:.4f})") @@ -238,11 +306,13 @@ def main(): print(f"Elevation gain: {elev_gain:.0f} m") print(f"Elevation loss: {elev_loss:.0f} m") print(f"Elevation range: {np.min(elev_arr):.0f} - {np.max(elev_arr):.0f} m") + if len(valid_fric) > 0: + print(f"Friction (path): min={valid_fric.min()}, max={valid_fric.max()}, mean={valid_fric.mean():.1f}") print(f"Path cells: {len(path_indices):,}") print(f"Wall time: {t_end - t0:.1f}s") - print(f"\nOutput saved to: {OUTPUT_PATH}") + print(f"\nOutput saved to: {OUTPUT_PATH_FRICTION}") - # Validation checks + # Validation print(f"\n" + "-" * 60) print("VALIDATION") print("-" * 60) @@ -258,15 +328,57 @@ def main(): is_nontrivial = len(path_indices) > 10 and total_distance_m > 1000 print(f"Path is non-trivial: {'PASS' if is_nontrivial else 'FAIL'}") - # Check it's not a straight line (measure sinuosity) + # Check sinuosity straight_line_dist = np.sqrt( (coordinates[-1][0] - coordinates[0][0])**2 + (coordinates[-1][1] - coordinates[0][1])**2 - ) * 111000 # rough degrees to meters + ) * 111000 sinuosity = total_distance_m / max(straight_line_dist, 1) print(f"Sinuosity: {sinuosity:.2f} (>1.0 means path curves around obstacles)") - reader.close() + # CRITICAL: Check no water cells (friction=255) on path + # This is the authoritative test - friction layer prevents water crossings + print(f"\n--- Water Avoidance Check ---") + water_on_path = np.sum(fric_arr == 255) + if water_on_path > 0: + print(f"FAIL: Path crosses {water_on_path} water cells (friction=255)") + sys.exit(1) + else: + print(f"PASS: No water cells (friction=255) on path") + + # Informational: Check if path goes through lake bounding box + # Path may go through land cells within the bbox, which is fine + print(f"\n--- Lake Bounding Box Check (informational) ---") + print(f"Murtaugh Lake bounds: {LAKE_BOUNDS}") + crosses_lake, crossing_point = path_crosses_lake(coordinates, LAKE_BOUNDS) + if crosses_lake: + print(f"INFO: Path passes through lake bbox at {crossing_point}") + print(f" (This is OK if friction check passed - path uses land cells)") + else: + print(f"PASS: Path does not enter lake bounding box") + + # Compare with Phase O1 if available + print(f"\n" + "-" * 60) + print("COMPARISON: Phase O1 vs O2b") + print("-" * 60) + + if OUTPUT_PATH_O1.exists(): + with open(OUTPUT_PATH_O1) as f: + o1_data = json.load(f) + o1_props = o1_data["properties"] + + print(f"{'Metric':<20} {'O1 (no friction)':<20} {'O2b (with friction)':<20}") + print("-" * 60) + print(f"{'Distance (km)':<20} {o1_props['total_distance_km']:<20.2f} {total_distance_m/1000:<20.2f}") + print(f"{'Effort (min)':<20} {o1_props['total_time_minutes']:<20.1f} {end_cost/60:<20.1f}") + print(f"{'Cell count':<20} {o1_props['cell_count']:<20} {len(path_indices):<20}") + print(f"{'Elev gain (m)':<20} {o1_props['elevation_gain_m']:<20.0f} {elev_gain:<20.0f}") + else: + print(f"Phase O1 output not found at {OUTPUT_PATH_O1}") + print(f"Run the O1 prototype first to enable comparison.") + + dem_reader.close() + friction_reader.close() print("\nPrototype completed successfully.") From e0eedcedfde5672fa3a61efe286ceb0082a6a838 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 06:53:11 +0000 Subject: [PATCH 03/16] =?UTF-8?q?feat(offroute):=20Phase=20O2c=20=E2=80=94?= =?UTF-8?q?=20PAD-US=20barriers=20with=20three-mode=20boundary=20respect?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add barriers.py: PAD-US raster reader + build_barriers_raster() function - Rasterize PAD-US Pub_Access=XA (Closed) polygons to CONUS GeoTIFF - Modify cost.py: boundary_mode parameter (strict/pragmatic/emergency) - strict: private land = impassable (np.inf) - pragmatic: private land = 5x friction penalty (default) - emergency: private land barriers ignored - Modify prototype.py: three-way comparison output - Output: padus_barriers.tif at /mnt/nav/worldcover/ (144MB, ~33m resolution) Co-Authored-By: Claude Opus 4.5 --- lib/offroute/barriers.py | 266 ++++++++++++++++++++ lib/offroute/cost.py | 310 ++++++++++++++---------- lib/offroute/prototype.py | 494 +++++++++++++++++++------------------- 3 files changed, 694 insertions(+), 376 deletions(-) create mode 100644 lib/offroute/barriers.py diff --git a/lib/offroute/barriers.py b/lib/offroute/barriers.py new file mode 100644 index 0000000..7fcad75 --- /dev/null +++ b/lib/offroute/barriers.py @@ -0,0 +1,266 @@ +""" +PAD-US barrier layer for OFFROUTE. + +Provides access to the PAD-US land ownership raster for routing decisions. +Cells with value 255 represent closed/restricted areas (Pub_Access = XA). + +Build function rasterizes PAD-US geodatabase to aligned GeoTIFF. +Runtime functions read the raster and resample to match elevation grids. +""" +import numpy as np +from pathlib import Path +from typing import Tuple, Optional +import subprocess +import tempfile +import os + +try: + import rasterio + from rasterio.windows import from_bounds + from rasterio.enums import Resampling +except ImportError: + raise ImportError("rasterio is required for barriers layer support") + +# Paths +DEFAULT_BARRIERS_PATH = Path("/mnt/nav/worldcover/padus_barriers.tif") +PADUS_GDB_PATH = Path("/mnt/nav/padus/PADUS4_0_Geodatabase.gdb") +PADUS_LAYER = "PADUS4_0Combined_Proclamation_Marine_Fee_Designation_Easement" + +# CONUS bounding box in WGS84 +CONUS_BOUNDS = { + "west": -125.0, + "east": -66.0, + "south": 24.0, + "north": 50.0, +} + +# Resolution in degrees (~30m at mid-latitudes) +PIXEL_SIZE = 0.0003 # ~33m + + +class BarrierReader: + """Reader for PAD-US barrier raster.""" + + def __init__(self, barrier_path: Path = DEFAULT_BARRIERS_PATH): + self.barrier_path = barrier_path + self._dataset = None + + def _open(self): + """Lazy open the dataset.""" + if self._dataset is None: + if not self.barrier_path.exists(): + raise FileNotFoundError( + f"Barrier raster not found at {self.barrier_path}. " + f"Run build_barriers_raster() first." + ) + self._dataset = rasterio.open(self.barrier_path) + return self._dataset + + def get_barrier_grid( + self, + south: float, + north: float, + west: float, + east: float, + target_shape: Tuple[int, int] + ) -> np.ndarray: + """ + Get barrier values for a bounding box, resampled to target shape. + + Args: + south, north, west, east: Bounding box coordinates (WGS84) + target_shape: (rows, cols) to resample to (matches elevation grid) + + Returns: + np.ndarray of uint8 barrier values: + 255 = closed/restricted (impassable when respect_boundaries=True) + 0 = public/accessible + """ + ds = self._open() + + # Create a window from the bounding box + window = from_bounds(west, south, east, north, ds.transform) + + # Read with resampling to target shape + barriers = ds.read( + 1, + window=window, + out_shape=target_shape, + resampling=Resampling.nearest + ) + + return barriers + + def sample_point(self, lat: float, lon: float) -> int: + """Sample barrier value at a single point.""" + ds = self._open() + + # Get pixel coordinates + row, col = ds.index(lon, lat) + + # Check bounds + if row < 0 or row >= ds.height or col < 0 or col >= ds.width: + return 0 # Out of bounds = accessible + + # Read single pixel + window = rasterio.windows.Window(col, row, 1, 1) + value = ds.read(1, window=window) + return int(value[0, 0]) + + def close(self): + """Close the dataset.""" + if self._dataset is not None: + self._dataset.close() + self._dataset = None + + +def build_barriers_raster( + output_path: Path = DEFAULT_BARRIERS_PATH, + gdb_path: Path = PADUS_GDB_PATH, + pixel_size: float = PIXEL_SIZE, + bounds: dict = CONUS_BOUNDS, +) -> Path: + """ + Build the PAD-US barriers raster from the source geodatabase. + + Extracts polygons where Pub_Access = 'XA' (Closed) and rasterizes them. + + Args: + output_path: Output GeoTIFF path + gdb_path: Path to PAD-US geodatabase + pixel_size: Pixel size in degrees + bounds: CONUS bounding box + + Returns: + Path to the created raster + """ + import shutil + + if not gdb_path.exists(): + raise FileNotFoundError(f"PAD-US geodatabase not found at {gdb_path}") + + # Check for required tools + if not shutil.which('ogr2ogr'): + raise RuntimeError("ogr2ogr not found. Install GDAL.") + if not shutil.which('gdal_rasterize'): + raise RuntimeError("gdal_rasterize not found. Install GDAL.") + + output_path.parent.mkdir(parents=True, exist_ok=True) + + print(f"Building PAD-US barriers raster...") + print(f" Source: {gdb_path}") + print(f" Output: {output_path}") + print(f" Pixel size: {pixel_size} degrees (~{pixel_size * 111000:.0f}m)") + print(f" Bounds: {bounds}") + + with tempfile.TemporaryDirectory() as tmpdir: + # Step 1: Extract closed areas and reproject to WGS84 + closed_gpkg = Path(tmpdir) / "closed_areas.gpkg" + + print(f"\n[1/3] Extracting closed areas (Pub_Access = 'XA')...") + + ogr_cmd = [ + "ogr2ogr", + "-f", "GPKG", + str(closed_gpkg), + str(gdb_path), + PADUS_LAYER, + "-where", "Pub_Access = 'XA'", + "-t_srs", "EPSG:4326", + "-nlt", "MULTIPOLYGON", + "-nln", "closed_areas", + ] + + result = subprocess.run(ogr_cmd, capture_output=True, text=True) + if result.returncode != 0: + print(f"STDERR: {result.stderr}") + raise RuntimeError(f"ogr2ogr failed: {result.stderr}") + + # Check feature count + info_cmd = ["ogrinfo", "-so", str(closed_gpkg), "closed_areas"] + info_result = subprocess.run(info_cmd, capture_output=True, text=True) + print(f" Extraction result:\n{info_result.stdout}") + + # Step 2: Create empty raster + print(f"\n[2/3] Creating raster grid...") + + width = int((bounds['east'] - bounds['west']) / pixel_size) + height = int((bounds['north'] - bounds['south']) / pixel_size) + + print(f" Grid size: {width} x {height} pixels") + print(f" Memory estimate: {width * height / 1e6:.1f} MB") + + # Step 3: Rasterize + print(f"\n[3/3] Rasterizing closed areas...") + + rasterize_cmd = [ + "gdal_rasterize", + "-burn", "255", + "-init", "0", + "-a_nodata", "0", # No nodata - 0 means accessible + "-te", str(bounds['west']), str(bounds['south']), + str(bounds['east']), str(bounds['north']), + "-tr", str(pixel_size), str(pixel_size), + "-ot", "Byte", + "-co", "COMPRESS=LZW", + "-co", "TILED=YES", + "-l", "closed_areas", + str(closed_gpkg), + str(output_path), + ] + + result = subprocess.run(rasterize_cmd, capture_output=True, text=True) + if result.returncode != 0: + print(f"STDERR: {result.stderr}") + raise RuntimeError(f"gdal_rasterize failed: {result.stderr}") + + # Verify output + print(f"\n[Done] Verifying output...") + with rasterio.open(output_path) as ds: + print(f" Size: {ds.width} x {ds.height}") + print(f" CRS: {ds.crs}") + print(f" Bounds: {ds.bounds}") + + # Sample a few tiles to check + sample = ds.read(1, window=rasterio.windows.Window(0, 0, 1000, 1000)) + closed_count = np.sum(sample == 255) + print(f" Sample (1000x1000): {closed_count} closed cells") + + file_size = output_path.stat().st_size / (1024**2) + print(f" File size: {file_size:.1f} MB") + + return output_path + + +if __name__ == "__main__": + import sys + + if len(sys.argv) > 1 and sys.argv[1] == "build": + # Build the raster + print("="*60) + print("PAD-US Barriers Raster Build") + print("="*60) + build_barriers_raster() + else: + # Test the reader + print("Testing BarrierReader...") + + if not DEFAULT_BARRIERS_PATH.exists(): + print(f"Barrier raster not found at {DEFAULT_BARRIERS_PATH}") + print(f"Run: python barriers.py build") + sys.exit(1) + + reader = BarrierReader() + + # Test grid read for Idaho area + barriers = reader.get_barrier_grid( + south=42.2, north=42.6, west=-114.8, east=-113.8, + target_shape=(400, 1000) + ) + print(f"\nGrid test shape: {barriers.shape}") + print(f"Unique values: {np.unique(barriers)}") + closed_cells = np.sum(barriers == 255) + print(f"Closed cells: {closed_cells} ({100*closed_cells/barriers.size:.2f}%)") + + reader.close() + print("\nBarrierReader test complete.") diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py index 3607de6..f31b8f5 100644 --- a/lib/offroute/cost.py +++ b/lib/offroute/cost.py @@ -1,132 +1,178 @@ -""" -Tobler off-path hiking cost function for OFFROUTE. - -Computes travel time cost based on terrain slope using Tobler's -hiking function with off-trail penalty. Optionally applies friction -multipliers from land cover data. -""" -import math -import numpy as np -from typing import Optional - -# Maximum passable slope in degrees -MAX_SLOPE_DEG = 40.0 - -# Tobler off-path parameters -TOBLER_BASE_SPEED = 6.0 -TOBLER_OFF_TRAIL_MULT = 0.6 - - -def tobler_speed(grade: float) -> float: - """ - Calculate hiking speed using Tobler's off-path function. - - speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) - - Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). - """ - return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) - - -def compute_cost_grid( - elevation: np.ndarray, - cell_size_m: float, - cell_size_lat_m: float = None, - cell_size_lon_m: float = None, - friction: Optional[np.ndarray] = None -) -> np.ndarray: - """ - Compute isotropic travel cost grid from elevation data. - - Each cell's cost represents the time (in seconds) to traverse that cell, - based on the average slope from neighboring cells. - - Args: - elevation: 2D array of elevation values in meters - cell_size_m: Average cell size in meters - cell_size_lat_m: Cell size in latitude direction (optional) - cell_size_lon_m: Cell size in longitude direction (optional) - friction: Optional 2D array of friction multipliers. - Values should be float (1.0 = baseline, 2.0 = 2x slower). - np.inf marks impassable cells. - If None, no friction is applied (backward compatible). - - Returns: - 2D array of travel cost in seconds per cell. - np.inf for impassable cells. - """ - if cell_size_lat_m is None: - cell_size_lat_m = cell_size_m - if cell_size_lon_m is None: - cell_size_lon_m = cell_size_m - - rows, cols = elevation.shape - - # Compute gradients in both directions - dy = np.zeros_like(elevation) - dx = np.zeros_like(elevation) - - # Central differences for interior, forward/backward at edges - dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) - dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m - dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m - - dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) - dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m - dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m - - # Compute slope magnitude (grade = rise/run) - grade_magnitude = np.sqrt(dx**2 + dy**2) - - # Convert to slope angle in degrees - slope_deg = np.degrees(np.arctan(grade_magnitude)) - - # Compute speed for each cell using Tobler function - speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) - - # Convert speed to time cost (seconds to traverse one cell) - avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 - cost = avg_cell_size * 3.6 / speed_kmh - - # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity - cost[slope_deg > MAX_SLOPE_DEG] = np.inf - - # Handle NaN elevations (no data) - cost[np.isnan(elevation)] = np.inf - - # Apply friction multipliers if provided - if friction is not None: - if friction.shape != elevation.shape: - raise ValueError( - f"Friction shape {friction.shape} does not match elevation shape {elevation.shape}" - ) - # Multiply cost by friction (inf * anything = inf, which is correct) - cost = cost * friction - - return cost - - -if __name__ == "__main__": - print("Testing Tobler speed function:") - for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: - speed = tobler_speed(grade) - print(f" Grade {grade:+.2f}: {speed:.2f} km/h") - - print("\nTesting cost grid computation (no friction):") - elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 - cost = compute_cost_grid(elev, cell_size_m=30.0) - print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") - finite = cost[~np.isinf(cost)] - if len(finite) > 0: - print(f" Cost range: {finite.min():.1f} - {finite.max():.1f} s") - else: - print(f" All cells impassable (test data too steep)") - - print("\nTesting cost grid with friction:") - elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain - friction = np.ones((10, 10), dtype=np.float32) * 1.5 # 1.5x friction - friction[5, 5] = np.inf # one impassable cell - cost = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) - print(f" Base cost (flat, 30m cell): {30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)):.1f} s") - print(f" With 1.5x friction: {cost[0, 0]:.1f} s") - print(f" Impassable cells: {np.sum(np.isinf(cost))}") +""" +Tobler off-path hiking cost function for OFFROUTE. + +Computes travel time cost based on terrain slope using Tobler's +hiking function with off-trail penalty. Optionally applies friction +multipliers from land cover data and barrier grids from PAD-US. +""" +import math +import numpy as np +from typing import Optional, Literal + +# Maximum passable slope in degrees +MAX_SLOPE_DEG = 40.0 + +# Tobler off-path parameters +TOBLER_BASE_SPEED = 6.0 +TOBLER_OFF_TRAIL_MULT = 0.6 + +# Pragmatic mode friction multiplier for private land +PRAGMATIC_BARRIER_MULTIPLIER = 5.0 + + +def tobler_speed(grade: float) -> float: + """ + Calculate hiking speed using Tobler's off-path function. + + speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) + + Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). + """ + return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) + + +def compute_cost_grid( + elevation: np.ndarray, + cell_size_m: float, + cell_size_lat_m: float = None, + cell_size_lon_m: float = None, + friction: Optional[np.ndarray] = None, + barriers: Optional[np.ndarray] = None, + boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" +) -> np.ndarray: + """ + Compute isotropic travel cost grid from elevation data. + + Each cell's cost represents the time (in seconds) to traverse that cell, + based on the average slope from neighboring cells. + + Args: + elevation: 2D array of elevation values in meters + cell_size_m: Average cell size in meters + cell_size_lat_m: Cell size in latitude direction (optional) + cell_size_lon_m: Cell size in longitude direction (optional) + friction: Optional 2D array of friction multipliers. + Values should be float (1.0 = baseline, 2.0 = 2x slower). + np.inf marks impassable cells. + If None, no friction is applied (backward compatible). + barriers: Optional 2D array of barrier values (uint8). + 255 = closed/restricted area (from PAD-US Pub_Access = XA). + 0 = accessible. + If None, no barriers are applied. + boundary_mode: How to handle private/restricted land barriers: + "strict" - cells with barrier=255 become impassable (np.inf) + "pragmatic" - cells with barrier=255 get 5.0x friction penalty + "emergency" - barriers are ignored entirely + Default: "pragmatic" + + Returns: + 2D array of travel cost in seconds per cell. + np.inf for impassable cells. + """ + if boundary_mode not in ("strict", "pragmatic", "emergency"): + raise ValueError(f"boundary_mode must be 'strict', 'pragmatic', or 'emergency', got '{boundary_mode}'") + + if cell_size_lat_m is None: + cell_size_lat_m = cell_size_m + if cell_size_lon_m is None: + cell_size_lon_m = cell_size_m + + rows, cols = elevation.shape + + # Compute gradients in both directions + dy = np.zeros_like(elevation) + dx = np.zeros_like(elevation) + + # Central differences for interior, forward/backward at edges + dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) + dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m + dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m + + dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) + dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m + dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m + + # Compute slope magnitude (grade = rise/run) + grade_magnitude = np.sqrt(dx**2 + dy**2) + + # Convert to slope angle in degrees + slope_deg = np.degrees(np.arctan(grade_magnitude)) + + # Compute speed for each cell using Tobler function + speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) + + # Convert speed to time cost (seconds to traverse one cell) + avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 + cost = avg_cell_size * 3.6 / speed_kmh + + # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity + cost[slope_deg > MAX_SLOPE_DEG] = np.inf + + # Handle NaN elevations (no data) + cost[np.isnan(elevation)] = np.inf + + # Apply friction multipliers if provided + if friction is not None: + if friction.shape != elevation.shape: + raise ValueError( + f"Friction shape {friction.shape} does not match elevation shape {elevation.shape}" + ) + # Multiply cost by friction (inf * anything = inf, which is correct) + cost = cost * friction + + # Apply barriers based on boundary_mode + if barriers is not None and boundary_mode != "emergency": + if barriers.shape != elevation.shape: + raise ValueError( + f"Barriers shape {barriers.shape} does not match elevation shape {elevation.shape}" + ) + + barrier_mask = barriers == 255 + + if boundary_mode == "strict": + # Mark closed/restricted areas as impassable + cost[barrier_mask] = np.inf + elif boundary_mode == "pragmatic": + # Apply friction penalty to closed/restricted areas + cost[barrier_mask] = cost[barrier_mask] * PRAGMATIC_BARRIER_MULTIPLIER + + return cost + + +if __name__ == "__main__": + print("Testing Tobler speed function:") + for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: + speed = tobler_speed(grade) + print(f" Grade {grade:+.2f}: {speed:.2f} km/h") + + print("\nTesting cost grid computation (no friction, no barriers):") + elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 + cost = compute_cost_grid(elev, cell_size_m=30.0) + print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") + finite = cost[~np.isinf(cost)] + if len(finite) > 0: + print(f" Cost range: {finite.min():.1f} - {finite.max():.1f} s") + else: + print(f" All cells impassable (test data too steep)") + + print("\nTesting cost grid with friction:") + elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain + friction = np.ones((10, 10), dtype=np.float32) * 1.5 # 1.5x friction + friction[5, 5] = np.inf # one impassable cell + cost = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) + print(f" Base cost (flat, 30m cell): {30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)):.1f} s") + print(f" With 1.5x friction: {cost[0, 0]:.1f} s") + print(f" Impassable cells: {np.sum(np.isinf(cost))}") + + print("\nTesting cost grid with barriers (three modes):") + elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain + barriers = np.zeros((10, 10), dtype=np.uint8) + barriers[3:7, 3:7] = 255 # 4x4 closed area in center + + base_cost = 30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)) + + for mode in ["strict", "pragmatic", "emergency"]: + cost = compute_cost_grid(elev, cell_size_m=30.0, barriers=barriers, boundary_mode=mode) + impassable = np.sum(np.isinf(cost)) + barrier_cost = cost[5, 5] if not np.isinf(cost[5, 5]) else "inf" + print(f" {mode:10s}: {impassable} impassable, barrier cell cost = {barrier_cost}") diff --git a/lib/offroute/prototype.py b/lib/offroute/prototype.py index 9822021..b5caf86 100755 --- a/lib/offroute/prototype.py +++ b/lib/offroute/prototype.py @@ -1,18 +1,16 @@ #!/usr/bin/env python3 """ -OFFROUTE Phase O2b Prototype +OFFROUTE Phase O2c Prototype -Validates the PMTiles decoder, Tobler cost function, WorldCover friction -integration, and MCP pathfinder on a real Idaho bounding box. +Validates the PMTiles decoder, Tobler cost function, WorldCover friction, +PAD-US barriers integration, and MCP pathfinder on a real Idaho bounding box. -Now includes friction layer to avoid water bodies like Murtaugh Lake. +Runs THREE pathfinding passes with different boundary modes: + 1. boundary_mode="strict" - private land is impassable + 2. boundary_mode="pragmatic" - private land has 5x friction penalty + 3. boundary_mode="emergency" - private land barriers ignored -Test bbox (four Idaho towns as corners): - SW: Rogerson, ID (~42.21, -114.60) - NW: Buhl, ID (~42.60, -114.76) - NE: Burley, ID (~42.54, -113.79) - SE: Oakley, ID (~42.24, -113.88) - Approximate bbox: south=42.21, north=42.60, west=-114.76, east=-113.79 +Outputs comparison showing impact of boundary mode on routing. """ import json import time @@ -28,8 +26,9 @@ sys.path.insert(0, str(Path(__file__).parent.parent.parent)) from lib.offroute.dem import DEMReader from lib.offroute.cost import compute_cost_grid from lib.offroute.friction import FrictionReader, friction_to_multiplier +from lib.offroute.barriers import BarrierReader, DEFAULT_BARRIERS_PATH -# Test bounding box +# Test bounding box - Idaho area known to have mixed public/private land BBOX = { "south": 42.21, "north": 42.60, @@ -38,26 +37,25 @@ BBOX = { } # Start point: wilderness area south of Twin Falls -# (in the Sawtooth National Forest foothills) -START_LAT = 42.35 -START_LON = -114.50 +START_LAT = 42.36 +START_LON = -114.55 # End point: near Burley, ID (on road network) -END_LAT = 42.52 -END_LON = -113.85 - -# Murtaugh Lake - actual water extent from WorldCover -LAKE_BOUNDS = { - "south": 42.44, - "north": 42.50, - "west": -114.20, - "east": -114.10, -} -LAKE_CENTER = (42.465, -114.155) # Verified water in WorldCover +END_LAT = 42.55 +END_LON = -114.25 # Output files -OUTPUT_PATH_O1 = Path("/opt/recon/data/offroute-test.geojson") -OUTPUT_PATH_FRICTION = Path("/opt/recon/data/offroute-test-friction.geojson") +OUTPUT_PATHS = { + "strict": Path("/opt/recon/data/offroute-test-strict.geojson"), + "pragmatic": Path("/opt/recon/data/offroute-test-pragmatic.geojson"), + "emergency": Path("/opt/recon/data/offroute-test-emergency.geojson"), +} + +# Old files to delete +OLD_FILES = [ + Path("/opt/recon/data/offroute-test-barriers-on.geojson"), + Path("/opt/recon/data/offroute-test-barriers-off.geojson"), +] # Memory limit in GB MEMORY_LIMIT_GB = 12 @@ -77,40 +75,139 @@ def check_memory_usage(): return 0 -def path_crosses_lake(coordinates, lake_bounds): - """Check if any path coordinates fall within the lake bounding box.""" - for lon, lat in coordinates: - if (lake_bounds["south"] <= lat <= lake_bounds["north"] and - lake_bounds["west"] <= lon <= lake_bounds["east"]): - return True, (lat, lon) - return False, None +def run_pathfinder( + elevation: np.ndarray, + meta: dict, + friction_mult: np.ndarray, + barriers: np.ndarray, + boundary_mode: str, + start_row: int, + start_col: int, + end_row: int, + end_col: int, + dem_reader: DEMReader, +) -> dict: + """ + Run the MCP pathfinder with given parameters. + + Returns dict with path info and stats. + """ + # Compute cost grid + cost = compute_cost_grid( + elevation, + cell_size_m=meta["cell_size_m"], + friction=friction_mult, + barriers=barriers, + boundary_mode=boundary_mode, + ) + + # Count impassable cells + impassable_count = np.sum(np.isinf(cost)) + barrier_count = np.sum(barriers == 255) if barriers is not None else 0 + + # Run MCP + mcp = MCP_Geometric(cost, fully_connected=True) + cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) + + end_cost = cumulative_costs[end_row, end_col] + + if np.isinf(end_cost): + return { + "success": False, + "reason": "No path found (blocked by impassable terrain)", + "impassable_cells": int(impassable_count), + "barrier_cells": int(barrier_count), + } + + # Traceback path + path_indices = mcp.traceback((end_row, end_col)) + + # Convert to coordinates + coordinates = [] + elevations = [] + barrier_values = [] + + for row, col in path_indices: + lat, lon = dem_reader.pixel_to_latlon(row, col, meta) + elev = elevation[row, col] + barr = barriers[row, col] if barriers is not None else 0 + coordinates.append([lon, lat]) + elevations.append(elev) + barrier_values.append(barr) + + # Compute distance + total_distance_m = 0 + for i in range(1, len(coordinates)): + lon1, lat1 = coordinates[i-1] + lon2, lat2 = coordinates[i] + R = 6371000 + dlat = np.radians(lat2 - lat1) + dlon = np.radians(lon2 - lon1) + a = np.sin(dlat/2)**2 + np.cos(np.radians(lat1)) * np.cos(np.radians(lat2)) * np.sin(dlon/2)**2 + c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1-a)) + total_distance_m += R * c + + # Elevation stats + elev_arr = np.array(elevations) + elev_diff = np.diff(elev_arr) + elev_gain = np.sum(elev_diff[elev_diff > 0]) + elev_loss = np.sum(np.abs(elev_diff[elev_diff < 0])) + + # Barrier crossings on path + barr_arr = np.array(barrier_values) + barrier_crossings = np.sum(barr_arr == 255) + + return { + "success": True, + "coordinates": coordinates, + "total_time_seconds": float(end_cost), + "total_time_minutes": float(end_cost / 60), + "total_distance_m": float(total_distance_m), + "total_distance_km": float(total_distance_m / 1000), + "elevation_gain_m": float(elev_gain), + "elevation_loss_m": float(elev_loss), + "min_elevation_m": float(np.min(elev_arr)), + "max_elevation_m": float(np.max(elev_arr)), + "cell_count": len(path_indices), + "impassable_cells": int(impassable_count), + "barrier_cells": int(barrier_count), + "barrier_crossings": int(barrier_crossings), + } def main(): - print("=" * 60) - print("OFFROUTE Phase O2b Prototype (with Friction)") - print("=" * 60) + print("=" * 80) + print("OFFROUTE Phase O2c Prototype (Three-Mode Boundary Respect)") + print("=" * 80) t0 = time.time() + # Delete old output files + for old_file in OLD_FILES: + if old_file.exists(): + old_file.unlink() + print(f"Deleted old file: {old_file}") + + # Check if barrier raster exists + if not DEFAULT_BARRIERS_PATH.exists(): + print(f"\nERROR: Barrier raster not found at {DEFAULT_BARRIERS_PATH}") + print(f"Run first: python /opt/recon/lib/offroute/barriers.py build") + sys.exit(1) + # Step 1: Load elevation data print(f"\n[1] Loading DEM for bbox: {BBOX}") dem_reader = DEMReader() - t1 = time.time() elevation, meta = dem_reader.get_elevation_grid( south=BBOX["south"], north=BBOX["north"], west=BBOX["west"], east=BBOX["east"], ) - t2 = time.time() print(f" Elevation grid shape: {elevation.shape}") print(f" Cell count: {elevation.size:,}") print(f" Cell size: {meta['cell_size_m']:.1f} m") - print(f" Elevation range: {np.nanmin(elevation):.0f} - {np.nanmax(elevation):.0f} m") - print(f" Load time: {t2 - t1:.1f}s") mem = check_memory_usage() if mem > 0: @@ -118,19 +215,8 @@ def main(): # Step 2: Load friction data print(f"\n[2] Loading WorldCover friction layer...") - t2a = time.time() - friction_reader = FrictionReader() - # Validate lake is marked as impassable - lake_friction = friction_reader.sample_point(LAKE_CENTER[0], LAKE_CENTER[1]) - print(f" Murtaugh Lake center ({LAKE_CENTER[0]}, {LAKE_CENTER[1]}): friction = {lake_friction}") - if lake_friction != 255: - print(f" WARNING: Lake not marked as water (expected 255, got {lake_friction})") - else: - print(f" Lake correctly marked as impassable (255)") - - # Load friction grid matching elevation shape friction_raw = friction_reader.get_friction_grid( south=BBOX["south"], north=BBOX["north"], @@ -138,36 +224,30 @@ def main(): east=BBOX["east"], target_shape=elevation.shape ) - t2b = time.time() - - # Convert to multipliers friction_mult = friction_to_multiplier(friction_raw) - impassable_count = np.sum(np.isinf(friction_mult)) print(f" Friction grid shape: {friction_raw.shape}") - print(f" Unique friction values: {np.unique(friction_raw[friction_raw > 0])}") - print(f" Impassable cells (water/nodata): {impassable_count:,} ({100*impassable_count/friction_raw.size:.1f}%)") - print(f" Load time: {t2b - t2a:.1f}s") + print(f" Water/impassable cells: {np.sum(np.isinf(friction_mult)):,}") - mem = check_memory_usage() - if mem > 0: - print(f" Memory usage: {mem:.1f} GB") + # Step 3: Load barrier data + print(f"\n[3] Loading PAD-US barrier layer...") + barrier_reader = BarrierReader() - # Step 3: Compute cost grid with friction - print(f"\n[3] Computing Tobler cost grid with friction...") - t3 = time.time() - cost = compute_cost_grid( - elevation, - cell_size_m=meta["cell_size_m"], - friction=friction_mult + barriers = barrier_reader.get_barrier_grid( + south=BBOX["south"], + north=BBOX["north"], + west=BBOX["west"], + east=BBOX["east"], + target_shape=elevation.shape ) - t4 = time.time() - finite_cost = cost[~np.isinf(cost)] - total_impassable = np.sum(np.isinf(cost)) - print(f" Cost range: {finite_cost.min():.1f} - {finite_cost.max():.1f} s/cell") - print(f" Total impassable cells: {total_impassable:,} ({100*total_impassable/cost.size:.1f}%)") - print(f" Compute time: {t4 - t3:.1f}s") + closed_cells = np.sum(barriers == 255) + print(f" Barrier grid shape: {barriers.shape}") + print(f" Closed/restricted cells: {closed_cells:,} ({100*closed_cells/barriers.size:.2f}%)") + + if closed_cells == 0: + print("\n WARNING: No closed/restricted areas in this bbox.") + print(" The test may not show meaningful differences between modes.") mem = check_memory_usage() if mem > 0: @@ -190,196 +270,122 @@ def main(): print(f"ERROR: End point outside grid bounds") sys.exit(1) - start_elev = elevation[start_row, start_col] - end_elev = elevation[end_row, end_col] - print(f" Start elevation: {start_elev:.0f} m") - print(f" End elevation: {end_elev:.0f} m") + # Step 5: Run pathfinder THREE times + results = {} + modes = ["strict", "pragmatic", "emergency"] - # Step 5: Run MCP pathfinder - print(f"\n[5] Running MCP_Geometric pathfinder...") - t5 = time.time() + for i, mode in enumerate(modes, start=5): + print(f"\n[{i}] Running pathfinder (boundary_mode=\"{mode}\")...") + t_start = time.time() + results[mode] = run_pathfinder( + elevation, meta, friction_mult, barriers, + boundary_mode=mode, + start_row=start_row, start_col=start_col, + end_row=end_row, end_col=end_col, + dem_reader=dem_reader, + ) + t_end = time.time() + print(f" Completed in {t_end - t_start:.1f}s") - mcp = MCP_Geometric(cost, fully_connected=True) - cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) - t6 = time.time() + # Step 6: Save GeoJSON outputs + print(f"\n[8] Saving GeoJSON outputs...") - print(f" Dijkstra completed in {t6 - t5:.1f}s") + OUTPUT_PATHS["strict"].parent.mkdir(parents=True, exist_ok=True) - end_cost = cumulative_costs[end_row, end_col] - print(f" Total cost to endpoint: {end_cost:.0f} seconds ({end_cost/60:.1f} minutes)") + for mode, result in results.items(): + output_path = OUTPUT_PATHS[mode] + if result["success"]: + geojson = { + "type": "Feature", + "properties": { + "type": f"offroute_{mode}", + "phase": "O2c", + "boundary_mode": mode, + "start": {"lat": START_LAT, "lon": START_LON}, + "end": {"lat": END_LAT, "lon": END_LON}, + **{k: v for k, v in result.items() if k not in ["success", "coordinates"]}, + }, + "geometry": { + "type": "LineString", + "coordinates": result["coordinates"], + } + } + with open(output_path, "w") as f: + json.dump(geojson, f, indent=2) + print(f" Saved: {output_path}") + else: + print(f" SKIPPED ({mode}): {result['reason']}") - if np.isinf(end_cost): - print("ERROR: No path found to endpoint (blocked by impassable terrain)") - sys.exit(1) + t_total = time.time() - t7 = time.time() - path_indices = mcp.traceback((end_row, end_col)) - t8 = time.time() + # Final report - three-way comparison + print(f"\n" + "=" * 80) + print("THREE-WAY COMPARISON") + print("=" * 80) - print(f" Traceback completed in {t8 - t7:.2f}s") - print(f" Path length: {len(path_indices)} cells") + # Check how many succeeded + success_count = sum(1 for r in results.values() if r["success"]) - mem = check_memory_usage() - if mem > 0: - print(f" Memory usage: {mem:.1f} GB") + if success_count == 3: + print(f"{'Metric':<22} {'STRICT':<18} {'PRAGMATIC':<18} {'EMERGENCY':<18}") + print("-" * 80) - # Step 6: Convert path to coordinates and compute stats - print(f"\n[6] Converting path to GeoJSON...") + metrics = [ + ("Distance (km)", "total_distance_km", ".2f"), + ("Effort time (min)", "total_time_minutes", ".1f"), + ("Cell count", "cell_count", "d"), + ("Elevation gain (m)", "elevation_gain_m", ".0f"), + ("Elevation loss (m)", "elevation_loss_m", ".0f"), + ("Barrier crossings", "barrier_crossings", "d"), + ("Impassable cells", "impassable_cells", ",d"), + ] - coordinates = [] - elevations = [] - friction_values = [] + for label, key, fmt in metrics: + vals = [results[m][key] for m in modes] + print(f"{label:<22} {vals[0]:<18{fmt}} {vals[1]:<18{fmt}} {vals[2]:<18{fmt}}") - for row, col in path_indices: - lat, lon = dem_reader.pixel_to_latlon(row, col, meta) - elev = elevation[row, col] - fric = friction_raw[row, col] - coordinates.append([lon, lat]) - elevations.append(elev) - friction_values.append(fric) + # Analysis + print(f"\n" + "-" * 80) + print("ANALYSIS") + print("-" * 80) - # Compute path distance - total_distance_m = 0 - for i in range(1, len(coordinates)): - lon1, lat1 = coordinates[i-1] - lon2, lat2 = coordinates[i] - R = 6371000 - dlat = np.radians(lat2 - lat1) - dlon = np.radians(lon2 - lon1) - a = np.sin(dlat/2)**2 + np.cos(np.radians(lat1)) * np.cos(np.radians(lat2)) * np.sin(dlon/2)**2 - c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1-a)) - total_distance_m += R * c + strict_crossings = results["strict"]["barrier_crossings"] + pragmatic_crossings = results["pragmatic"]["barrier_crossings"] + emergency_crossings = results["emergency"]["barrier_crossings"] - # Compute elevation gain/loss - elev_arr = np.array(elevations) - elev_diff = np.diff(elev_arr) - elev_gain = np.sum(elev_diff[elev_diff > 0]) - elev_loss = np.sum(np.abs(elev_diff[elev_diff < 0])) + print(f"Barrier crossings: strict={strict_crossings}, pragmatic={pragmatic_crossings}, emergency={emergency_crossings}") - # Friction stats along path - fric_arr = np.array(friction_values) - valid_fric = fric_arr[(fric_arr > 0) & (fric_arr < 255)] + if strict_crossings == 0 and pragmatic_crossings == 0 and emergency_crossings == 0: + print("No path crosses private land - terrain naturally avoids barriers.") + else: + if emergency_crossings > pragmatic_crossings: + print(f"Pragmatic mode reduces barrier crossings vs emergency: {emergency_crossings} -> {pragmatic_crossings}") + if pragmatic_crossings > 0 and strict_crossings == 0: + print(f"Strict mode completely avoids private land (pragmatic crosses {pragmatic_crossings} cells)") - # Build GeoJSON - geojson = { - "type": "Feature", - "properties": { - "type": "offroute_prototype_friction", - "phase": "O2b", - "start": {"lat": START_LAT, "lon": START_LON}, - "end": {"lat": END_LAT, "lon": END_LON}, - "total_time_seconds": float(end_cost), - "total_time_minutes": float(end_cost / 60), - "total_distance_m": float(total_distance_m), - "total_distance_km": float(total_distance_m / 1000), - "elevation_gain_m": float(elev_gain), - "elevation_loss_m": float(elev_loss), - "min_elevation_m": float(np.min(elev_arr)), - "max_elevation_m": float(np.max(elev_arr)), - "friction_min": int(valid_fric.min()) if len(valid_fric) > 0 else 0, - "friction_max": int(valid_fric.max()) if len(valid_fric) > 0 else 0, - "friction_mean": float(valid_fric.mean()) if len(valid_fric) > 0 else 0, - "cell_count": len(path_indices), - "cell_size_m": meta["cell_size_m"], - }, - "geometry": { - "type": "LineString", - "coordinates": coordinates, - } - } + # Time/distance comparison + if results["strict"]["total_time_minutes"] > results["emergency"]["total_time_minutes"]: + time_penalty = results["strict"]["total_time_minutes"] - results["emergency"]["total_time_minutes"] + print(f"Time cost of strict boundary respect: +{time_penalty:.1f} min") - # Write output - OUTPUT_PATH_FRICTION.parent.mkdir(parents=True, exist_ok=True) - with open(OUTPUT_PATH_FRICTION, "w") as f: - json.dump(geojson, f, indent=2) - - t_end = time.time() - - # Final report - print(f"\n" + "=" * 60) - print("RESULTS (Phase O2b with Friction)") - print("=" * 60) - print(f"Start: ({START_LAT:.4f}, {START_LON:.4f})") - print(f"End: ({END_LAT:.4f}, {END_LON:.4f})") - print(f"Total effort: {end_cost/60:.1f} minutes ({end_cost/3600:.2f} hours)") - print(f"Distance: {total_distance_m/1000:.2f} km") - print(f"Elevation gain: {elev_gain:.0f} m") - print(f"Elevation loss: {elev_loss:.0f} m") - print(f"Elevation range: {np.min(elev_arr):.0f} - {np.max(elev_arr):.0f} m") - if len(valid_fric) > 0: - print(f"Friction (path): min={valid_fric.min()}, max={valid_fric.max()}, mean={valid_fric.mean():.1f}") - print(f"Path cells: {len(path_indices):,}") - print(f"Wall time: {t_end - t0:.1f}s") - print(f"\nOutput saved to: {OUTPUT_PATH_FRICTION}") - - # Validation - print(f"\n" + "-" * 60) - print("VALIDATION") - print("-" * 60) - - # Check coordinates are within bbox - lons = [c[0] for c in coordinates] - lats = [c[1] for c in coordinates] - lon_ok = BBOX["west"] <= min(lons) and max(lons) <= BBOX["east"] - lat_ok = BBOX["south"] <= min(lats) and max(lats) <= BBOX["north"] - print(f"Coordinates within bbox: {'PASS' if lon_ok and lat_ok else 'FAIL'}") - - # Check path is not trivial - is_nontrivial = len(path_indices) > 10 and total_distance_m > 1000 - print(f"Path is non-trivial: {'PASS' if is_nontrivial else 'FAIL'}") - - # Check sinuosity - straight_line_dist = np.sqrt( - (coordinates[-1][0] - coordinates[0][0])**2 + - (coordinates[-1][1] - coordinates[0][1])**2 - ) * 111000 - sinuosity = total_distance_m / max(straight_line_dist, 1) - print(f"Sinuosity: {sinuosity:.2f} (>1.0 means path curves around obstacles)") - - # CRITICAL: Check no water cells (friction=255) on path - # This is the authoritative test - friction layer prevents water crossings - print(f"\n--- Water Avoidance Check ---") - water_on_path = np.sum(fric_arr == 255) - if water_on_path > 0: - print(f"FAIL: Path crosses {water_on_path} water cells (friction=255)") - sys.exit(1) else: - print(f"PASS: No water cells (friction=255) on path") + print(f"Only {success_count}/3 modes found a path:") + for mode, result in results.items(): + if result["success"]: + print(f" {mode}: {result['total_distance_km']:.2f} km, {result['total_time_minutes']:.1f} min") + else: + print(f" {mode}: FAILED - {result.get('reason', 'unknown')}") - # Informational: Check if path goes through lake bounding box - # Path may go through land cells within the bbox, which is fine - print(f"\n--- Lake Bounding Box Check (informational) ---") - print(f"Murtaugh Lake bounds: {LAKE_BOUNDS}") - crosses_lake, crossing_point = path_crosses_lake(coordinates, LAKE_BOUNDS) - if crosses_lake: - print(f"INFO: Path passes through lake bbox at {crossing_point}") - print(f" (This is OK if friction check passed - path uses land cells)") - else: - print(f"PASS: Path does not enter lake bounding box") - - # Compare with Phase O1 if available - print(f"\n" + "-" * 60) - print("COMPARISON: Phase O1 vs O2b") - print("-" * 60) - - if OUTPUT_PATH_O1.exists(): - with open(OUTPUT_PATH_O1) as f: - o1_data = json.load(f) - o1_props = o1_data["properties"] - - print(f"{'Metric':<20} {'O1 (no friction)':<20} {'O2b (with friction)':<20}") - print("-" * 60) - print(f"{'Distance (km)':<20} {o1_props['total_distance_km']:<20.2f} {total_distance_m/1000:<20.2f}") - print(f"{'Effort (min)':<20} {o1_props['total_time_minutes']:<20.1f} {end_cost/60:<20.1f}") - print(f"{'Cell count':<20} {o1_props['cell_count']:<20} {len(path_indices):<20}") - print(f"{'Elev gain (m)':<20} {o1_props['elevation_gain_m']:<20.0f} {elev_gain:<20.0f}") - else: - print(f"Phase O1 output not found at {OUTPUT_PATH_O1}") - print(f"Run the O1 prototype first to enable comparison.") + print(f"\n" + "-" * 80) + print(f"Total wall time: {t_total - t0:.1f}s") + print(f"Closed cells in bbox: {closed_cells:,}") + # Cleanup dem_reader.close() friction_reader.close() - print("\nPrototype completed successfully.") + barrier_reader.close() + + print("\nPrototype completed.") if __name__ == "__main__": From 3293cb4238a1ed489b9d6eb2259a7a4094b5331c Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 07:26:25 +0000 Subject: [PATCH 04/16] =?UTF-8?q?feat(offroute):=20Phase=20O3a=20=E2=80=94?= =?UTF-8?q?=20trail=20burn-in,=20pathfinder=20seeks=20trail=20corridors?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Trail friction REPLACES land cover friction where trails exist: - Road (value 5): 0.1× friction - Track (value 15): 0.3× friction - Foot trail (value 25): 0.5× friction TrailReader loads /mnt/nav/worldcover/trails.tif rasterized from OSM highways. Validation shows trail-seeking behavior: - On-trail travel: 17.3% → 98.7% - Effort time: 1047 min → 155 min (-85.2%) - Path travels farther but stays on roads for speed Co-Authored-By: Claude Opus 4.5 --- lib/offroute/cost.py | 70 ++++++--- lib/offroute/prototype.py | 302 ++++++++++++++++++++------------------ lib/offroute/trails.py | 174 ++++++++++++++++++++++ 3 files changed, 389 insertions(+), 157 deletions(-) create mode 100644 lib/offroute/trails.py diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py index f31b8f5..5f3618c 100644 --- a/lib/offroute/cost.py +++ b/lib/offroute/cost.py @@ -3,7 +3,7 @@ Tobler off-path hiking cost function for OFFROUTE. Computes travel time cost based on terrain slope using Tobler's hiking function with off-trail penalty. Optionally applies friction -multipliers from land cover data and barrier grids from PAD-US. +multipliers from land cover data, trail corridors, and barrier grids. """ import math import numpy as np @@ -19,6 +19,14 @@ TOBLER_OFF_TRAIL_MULT = 0.6 # Pragmatic mode friction multiplier for private land PRAGMATIC_BARRIER_MULTIPLIER = 5.0 +# Trail value to friction multiplier mapping +# Trail friction REPLACES land cover friction (a road through forest is still easy) +TRAIL_FRICTION_MAP = { + 5: 0.1, # road + 15: 0.3, # track + 25: 0.5, # foot trail +} + def tobler_speed(grade: float) -> float: """ @@ -37,6 +45,7 @@ def compute_cost_grid( cell_size_lat_m: float = None, cell_size_lon_m: float = None, friction: Optional[np.ndarray] = None, + trails: Optional[np.ndarray] = None, barriers: Optional[np.ndarray] = None, boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" ) -> np.ndarray: @@ -51,10 +60,17 @@ def compute_cost_grid( cell_size_m: Average cell size in meters cell_size_lat_m: Cell size in latitude direction (optional) cell_size_lon_m: Cell size in longitude direction (optional) - friction: Optional 2D array of friction multipliers. + friction: Optional 2D array of friction multipliers (WorldCover). Values should be float (1.0 = baseline, 2.0 = 2x slower). np.inf marks impassable cells. If None, no friction is applied (backward compatible). + trails: Optional 2D array of trail values (uint8). + 0 = no trail (use friction) + 5 = road (0.1× friction, replaces WorldCover) + 15 = track (0.3× friction, replaces WorldCover) + 25 = foot trail (0.5× friction, replaces WorldCover) + Trail friction REPLACES land cover friction where trails exist. + If None, no trail burn-in is applied. barriers: Optional 2D array of barrier values (uint8). 255 = closed/restricted area (from PAD-US Pub_Access = XA). 0 = accessible. @@ -111,14 +127,30 @@ def compute_cost_grid( # Handle NaN elevations (no data) cost[np.isnan(elevation)] = np.inf - # Apply friction multipliers if provided + # Build effective friction array + # Start with WorldCover friction if provided, else 1.0 if friction is not None: if friction.shape != elevation.shape: raise ValueError( f"Friction shape {friction.shape} does not match elevation shape {elevation.shape}" ) - # Multiply cost by friction (inf * anything = inf, which is correct) - cost = cost * friction + effective_friction = friction.copy() + else: + effective_friction = np.ones(elevation.shape, dtype=np.float32) + + # Apply trail burn-in: trails REPLACE land cover friction + if trails is not None: + if trails.shape != elevation.shape: + raise ValueError( + f"Trails shape {trails.shape} does not match elevation shape {elevation.shape}" + ) + # Replace friction where trails exist + for trail_value, trail_friction in TRAIL_FRICTION_MAP.items(): + trail_mask = trails == trail_value + effective_friction[trail_mask] = trail_friction + + # Apply friction to cost + cost = cost * effective_friction # Apply barriers based on boundary_mode if barriers is not None and boundary_mode != "emergency": @@ -145,7 +177,7 @@ if __name__ == "__main__": speed = tobler_speed(grade) print(f" Grade {grade:+.2f}: {speed:.2f} km/h") - print("\nTesting cost grid computation (no friction, no barriers):") + print("\nTesting cost grid computation (no friction, no trails):") elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 cost = compute_cost_grid(elev, cell_size_m=30.0) print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") @@ -155,21 +187,25 @@ if __name__ == "__main__": else: print(f" All cells impassable (test data too steep)") - print("\nTesting cost grid with friction:") + print("\nTesting cost grid with friction and trails:") elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain - friction = np.ones((10, 10), dtype=np.float32) * 1.5 # 1.5x friction - friction[5, 5] = np.inf # one impassable cell - cost = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) - print(f" Base cost (flat, 30m cell): {30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)):.1f} s") - print(f" With 1.5x friction: {cost[0, 0]:.1f} s") - print(f" Impassable cells: {np.sum(np.isinf(cost))}") + friction = np.ones((10, 10), dtype=np.float32) * 2.0 # 2.0x friction (forest) + trails = np.zeros((10, 10), dtype=np.uint8) + trails[5, :] = 5 # road across middle row - print("\nTesting cost grid with barriers (three modes):") - elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain - barriers = np.zeros((10, 10), dtype=np.uint8) - barriers[3:7, 3:7] = 255 # 4x4 closed area in center + cost_no_trail = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) + cost_with_trail = compute_cost_grid(elev, cell_size_m=30.0, friction=friction, trails=trails) base_cost = 30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)) + print(f" Base cost (flat, 30m cell): {base_cost:.1f} s") + print(f" Forest cell (2.0x friction): {cost_no_trail[0, 0]:.1f} s") + print(f" Road cell (0.1x friction, replaces forest): {cost_with_trail[5, 0]:.1f} s") + print(f" Road friction advantage: {cost_no_trail[0, 0] / cost_with_trail[5, 0]:.1f}x faster") + + print("\nTesting cost grid with barriers (three modes):") + elev = np.ones((10, 10), dtype=np.float32) * 1000 + barriers = np.zeros((10, 10), dtype=np.uint8) + barriers[3:7, 3:7] = 255 for mode in ["strict", "pragmatic", "emergency"]: cost = compute_cost_grid(elev, cell_size_m=30.0, barriers=barriers, boundary_mode=mode) diff --git a/lib/offroute/prototype.py b/lib/offroute/prototype.py index b5caf86..c9b78f0 100755 --- a/lib/offroute/prototype.py +++ b/lib/offroute/prototype.py @@ -1,16 +1,12 @@ #!/usr/bin/env python3 """ -OFFROUTE Phase O2c Prototype +OFFROUTE Phase O3a Prototype -Validates the PMTiles decoder, Tobler cost function, WorldCover friction, -PAD-US barriers integration, and MCP pathfinder on a real Idaho bounding box. +Validates trail burn-in integration with the MCP pathfinder. +The path should actively seek out trails and roads when nearby. -Runs THREE pathfinding passes with different boundary modes: - 1. boundary_mode="strict" - private land is impassable - 2. boundary_mode="pragmatic" - private land has 5x friction penalty - 3. boundary_mode="emergency" - private land barriers ignored - -Outputs comparison showing impact of boundary mode on routing. +Compares paths with and without trail burn-in to show the benefit +of trail-seeking behavior. """ import json import time @@ -27,8 +23,9 @@ from lib.offroute.dem import DEMReader from lib.offroute.cost import compute_cost_grid from lib.offroute.friction import FrictionReader, friction_to_multiplier from lib.offroute.barriers import BarrierReader, DEFAULT_BARRIERS_PATH +from lib.offroute.trails import TrailReader, DEFAULT_TRAILS_PATH -# Test bounding box - Idaho area known to have mixed public/private land +# Test bounding box - Idaho area BBOX = { "south": 42.21, "north": 42.60, @@ -36,26 +33,17 @@ BBOX = { "east": -113.79, } -# Start point: wilderness area south of Twin Falls -START_LAT = 42.36 -START_LON = -114.55 +# Start point: wilderness area away from roads +START_LAT = 42.35 +START_LON = -114.60 -# End point: near Burley, ID (on road network) +# End point: near Twin Falls (has roads/trails) END_LAT = 42.55 -END_LON = -114.25 +END_LON = -114.20 # Output files -OUTPUT_PATHS = { - "strict": Path("/opt/recon/data/offroute-test-strict.geojson"), - "pragmatic": Path("/opt/recon/data/offroute-test-pragmatic.geojson"), - "emergency": Path("/opt/recon/data/offroute-test-emergency.geojson"), -} - -# Old files to delete -OLD_FILES = [ - Path("/opt/recon/data/offroute-test-barriers-on.geojson"), - Path("/opt/recon/data/offroute-test-barriers-off.geojson"), -] +OUTPUT_PATH_WITH_TRAILS = Path("/opt/recon/data/offroute-test-trails.geojson") +OUTPUT_PATH_NO_TRAILS = Path("/opt/recon/data/offroute-test-no-trails.geojson") # Memory limit in GB MEMORY_LIMIT_GB = 12 @@ -79,32 +67,26 @@ def run_pathfinder( elevation: np.ndarray, meta: dict, friction_mult: np.ndarray, + trails: np.ndarray, barriers: np.ndarray, - boundary_mode: str, + use_trails: bool, start_row: int, start_col: int, end_row: int, end_col: int, dem_reader: DEMReader, ) -> dict: - """ - Run the MCP pathfinder with given parameters. - - Returns dict with path info and stats. - """ + """Run the MCP pathfinder with given parameters.""" # Compute cost grid cost = compute_cost_grid( elevation, cell_size_m=meta["cell_size_m"], friction=friction_mult, + trails=trails if use_trails else None, barriers=barriers, - boundary_mode=boundary_mode, + boundary_mode="pragmatic", ) - # Count impassable cells - impassable_count = np.sum(np.isinf(cost)) - barrier_count = np.sum(barriers == 255) if barriers is not None else 0 - # Run MCP mcp = MCP_Geometric(cost, fully_connected=True) cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) @@ -115,25 +97,23 @@ def run_pathfinder( return { "success": False, "reason": "No path found (blocked by impassable terrain)", - "impassable_cells": int(impassable_count), - "barrier_cells": int(barrier_count), } # Traceback path path_indices = mcp.traceback((end_row, end_col)) - # Convert to coordinates + # Convert to coordinates and collect stats coordinates = [] elevations = [] - barrier_values = [] + trail_values = [] for row, col in path_indices: lat, lon = dem_reader.pixel_to_latlon(row, col, meta) elev = elevation[row, col] - barr = barriers[row, col] if barriers is not None else 0 + trail_val = trails[row, col] if trails is not None else 0 coordinates.append([lon, lat]) elevations.append(elev) - barrier_values.append(barr) + trail_values.append(trail_val) # Compute distance total_distance_m = 0 @@ -153,9 +133,14 @@ def run_pathfinder( elev_gain = np.sum(elev_diff[elev_diff > 0]) elev_loss = np.sum(np.abs(elev_diff[elev_diff < 0])) - # Barrier crossings on path - barr_arr = np.array(barrier_values) - barrier_crossings = np.sum(barr_arr == 255) + # Trail stats + trail_arr = np.array(trail_values) + road_cells = np.sum(trail_arr == 5) + track_cells = np.sum(trail_arr == 15) + trail_cells = np.sum(trail_arr == 25) + off_trail_cells = np.sum(trail_arr == 0) + on_trail_cells = road_cells + track_cells + trail_cells + total_cells = len(trail_arr) return { "success": True, @@ -168,30 +153,28 @@ def run_pathfinder( "elevation_loss_m": float(elev_loss), "min_elevation_m": float(np.min(elev_arr)), "max_elevation_m": float(np.max(elev_arr)), - "cell_count": len(path_indices), - "impassable_cells": int(impassable_count), - "barrier_cells": int(barrier_count), - "barrier_crossings": int(barrier_crossings), + "cell_count": total_cells, + "road_cells": int(road_cells), + "track_cells": int(track_cells), + "trail_cells": int(trail_cells), + "off_trail_cells": int(off_trail_cells), + "on_trail_pct": float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0, } def main(): print("=" * 80) - print("OFFROUTE Phase O2c Prototype (Three-Mode Boundary Respect)") + print("OFFROUTE Phase O3a Prototype (Trail Burn-In)") print("=" * 80) t0 = time.time() - # Delete old output files - for old_file in OLD_FILES: - if old_file.exists(): - old_file.unlink() - print(f"Deleted old file: {old_file}") - - # Check if barrier raster exists + # Check for required rasters if not DEFAULT_BARRIERS_PATH.exists(): print(f"\nERROR: Barrier raster not found at {DEFAULT_BARRIERS_PATH}") - print(f"Run first: python /opt/recon/lib/offroute/barriers.py build") + sys.exit(1) + if not DEFAULT_TRAILS_PATH.exists(): + print(f"\nERROR: Trails raster not found at {DEFAULT_TRAILS_PATH}") sys.exit(1) # Step 1: Load elevation data @@ -243,25 +226,42 @@ def main(): closed_cells = np.sum(barriers == 255) print(f" Barrier grid shape: {barriers.shape}") - print(f" Closed/restricted cells: {closed_cells:,} ({100*closed_cells/barriers.size:.2f}%)") + print(f" Closed/restricted cells: {closed_cells:,}") - if closed_cells == 0: - print("\n WARNING: No closed/restricted areas in this bbox.") - print(" The test may not show meaningful differences between modes.") + # Step 4: Load trails data + print(f"\n[4] Loading OSM trails layer...") + trail_reader = TrailReader() + + trails = trail_reader.get_trails_grid( + south=BBOX["south"], + north=BBOX["north"], + west=BBOX["west"], + east=BBOX["east"], + target_shape=elevation.shape + ) + + road_cells = np.sum(trails == 5) + track_cells = np.sum(trails == 15) + trail_cells = np.sum(trails == 25) + print(f" Trails grid shape: {trails.shape}") + print(f" Road cells: {road_cells:,}") + print(f" Track cells: {track_cells:,}") + print(f" Trail cells: {trail_cells:,}") + print(f" Total trail coverage: {100*(road_cells+track_cells+trail_cells)/trails.size:.2f}%") mem = check_memory_usage() if mem > 0: print(f" Memory usage: {mem:.1f} GB") - # Step 4: Convert start/end to pixel coordinates - print(f"\n[4] Converting coordinates...") + # Step 5: Convert start/end to pixel coordinates + print(f"\n[5] Converting coordinates...") start_row, start_col = dem_reader.latlon_to_pixel(START_LAT, START_LON, meta) end_row, end_col = dem_reader.latlon_to_pixel(END_LAT, END_LON, meta) print(f" Start: ({START_LAT}, {START_LON}) -> pixel ({start_row}, {start_col})") print(f" End: ({END_LAT}, {END_LON}) -> pixel ({end_row}, {end_col})") - # Validate coordinates are within bounds + # Validate coordinates rows, cols = elevation.shape if not (0 <= start_row < rows and 0 <= start_col < cols): print(f"ERROR: Start point outside grid bounds") @@ -270,64 +270,86 @@ def main(): print(f"ERROR: End point outside grid bounds") sys.exit(1) - # Step 5: Run pathfinder THREE times - results = {} - modes = ["strict", "pragmatic", "emergency"] + # Step 6: Run pathfinder WITH trails + print(f"\n[6] Running pathfinder WITH trail burn-in...") + t6a = time.time() + result_trails = run_pathfinder( + elevation, meta, friction_mult, trails, barriers, + use_trails=True, + start_row=start_row, start_col=start_col, + end_row=end_row, end_col=end_col, + dem_reader=dem_reader, + ) + t6b = time.time() + print(f" Completed in {t6b - t6a:.1f}s") - for i, mode in enumerate(modes, start=5): - print(f"\n[{i}] Running pathfinder (boundary_mode=\"{mode}\")...") - t_start = time.time() - results[mode] = run_pathfinder( - elevation, meta, friction_mult, barriers, - boundary_mode=mode, - start_row=start_row, start_col=start_col, - end_row=end_row, end_col=end_col, - dem_reader=dem_reader, - ) - t_end = time.time() - print(f" Completed in {t_end - t_start:.1f}s") + # Step 7: Run pathfinder WITHOUT trails + print(f"\n[7] Running pathfinder WITHOUT trail burn-in...") + t7a = time.time() + result_no_trails = run_pathfinder( + elevation, meta, friction_mult, trails, barriers, + use_trails=False, + start_row=start_row, start_col=start_col, + end_row=end_row, end_col=end_col, + dem_reader=dem_reader, + ) + t7b = time.time() + print(f" Completed in {t7b - t7a:.1f}s") - # Step 6: Save GeoJSON outputs + # Step 8: Save GeoJSON outputs print(f"\n[8] Saving GeoJSON outputs...") - OUTPUT_PATHS["strict"].parent.mkdir(parents=True, exist_ok=True) + OUTPUT_PATH_WITH_TRAILS.parent.mkdir(parents=True, exist_ok=True) - for mode, result in results.items(): - output_path = OUTPUT_PATHS[mode] - if result["success"]: - geojson = { - "type": "Feature", - "properties": { - "type": f"offroute_{mode}", - "phase": "O2c", - "boundary_mode": mode, - "start": {"lat": START_LAT, "lon": START_LON}, - "end": {"lat": END_LAT, "lon": END_LON}, - **{k: v for k, v in result.items() if k not in ["success", "coordinates"]}, - }, - "geometry": { - "type": "LineString", - "coordinates": result["coordinates"], - } + if result_trails["success"]: + geojson = { + "type": "Feature", + "properties": { + "type": "offroute_with_trails", + "phase": "O3a", + "trail_burn_in": True, + "start": {"lat": START_LAT, "lon": START_LON}, + "end": {"lat": END_LAT, "lon": END_LON}, + **{k: v for k, v in result_trails.items() if k not in ["success", "coordinates"]}, + }, + "geometry": { + "type": "LineString", + "coordinates": result_trails["coordinates"], } - with open(output_path, "w") as f: - json.dump(geojson, f, indent=2) - print(f" Saved: {output_path}") - else: - print(f" SKIPPED ({mode}): {result['reason']}") + } + with open(OUTPUT_PATH_WITH_TRAILS, "w") as f: + json.dump(geojson, f, indent=2) + print(f" Saved: {OUTPUT_PATH_WITH_TRAILS}") + + if result_no_trails["success"]: + geojson = { + "type": "Feature", + "properties": { + "type": "offroute_no_trails", + "phase": "O3a", + "trail_burn_in": False, + "start": {"lat": START_LAT, "lon": START_LON}, + "end": {"lat": END_LAT, "lon": END_LON}, + **{k: v for k, v in result_no_trails.items() if k not in ["success", "coordinates"]}, + }, + "geometry": { + "type": "LineString", + "coordinates": result_no_trails["coordinates"], + } + } + with open(OUTPUT_PATH_NO_TRAILS, "w") as f: + json.dump(geojson, f, indent=2) + print(f" Saved: {OUTPUT_PATH_NO_TRAILS}") t_total = time.time() - # Final report - three-way comparison + # Final report print(f"\n" + "=" * 80) - print("THREE-WAY COMPARISON") + print("SIDE-BY-SIDE COMPARISON: Trail Burn-In Effect") print("=" * 80) - # Check how many succeeded - success_count = sum(1 for r in results.values() if r["success"]) - - if success_count == 3: - print(f"{'Metric':<22} {'STRICT':<18} {'PRAGMATIC':<18} {'EMERGENCY':<18}") + if result_trails["success"] and result_no_trails["success"]: + print(f"{'Metric':<25} {'WITH TRAILS':<20} {'WITHOUT TRAILS':<20} {'Delta':<15}") print("-" * 80) metrics = [ @@ -335,55 +357,55 @@ def main(): ("Effort time (min)", "total_time_minutes", ".1f"), ("Cell count", "cell_count", "d"), ("Elevation gain (m)", "elevation_gain_m", ".0f"), - ("Elevation loss (m)", "elevation_loss_m", ".0f"), - ("Barrier crossings", "barrier_crossings", "d"), - ("Impassable cells", "impassable_cells", ",d"), + ("On-trail %", "on_trail_pct", ".1f"), + ("Road cells", "road_cells", "d"), + ("Track cells", "track_cells", "d"), + ("Trail cells", "trail_cells", "d"), ] for label, key, fmt in metrics: - vals = [results[m][key] for m in modes] - print(f"{label:<22} {vals[0]:<18{fmt}} {vals[1]:<18{fmt}} {vals[2]:<18{fmt}}") + val_with = result_trails[key] + val_without = result_no_trails[key] + if isinstance(val_with, int): + delta = val_with - val_without + delta_str = f"{delta:+d}" + else: + delta = val_with - val_without + delta_str = f"{delta:+.2f}" + print(f"{label:<25} {val_with:<20{fmt}} {val_without:<20{fmt}} {delta_str:<15}") # Analysis print(f"\n" + "-" * 80) print("ANALYSIS") print("-" * 80) - strict_crossings = results["strict"]["barrier_crossings"] - pragmatic_crossings = results["pragmatic"]["barrier_crossings"] - emergency_crossings = results["emergency"]["barrier_crossings"] + time_saved = result_no_trails["total_time_minutes"] - result_trails["total_time_minutes"] + if time_saved > 0: + print(f"Trail burn-in saves {time_saved:.1f} minutes ({100*time_saved/result_no_trails['total_time_minutes']:.1f}% faster)") + elif time_saved < 0: + print(f"Trail burn-in adds {-time_saved:.1f} minutes (path seeks trails even if longer)") - print(f"Barrier crossings: strict={strict_crossings}, pragmatic={pragmatic_crossings}, emergency={emergency_crossings}") - - if strict_crossings == 0 and pragmatic_crossings == 0 and emergency_crossings == 0: - print("No path crosses private land - terrain naturally avoids barriers.") + on_trail_with = result_trails["on_trail_pct"] + on_trail_without = result_no_trails["on_trail_pct"] + if on_trail_with > on_trail_without: + print(f"Trail burn-in increases on-trail travel: {on_trail_without:.1f}% → {on_trail_with:.1f}%") else: - if emergency_crossings > pragmatic_crossings: - print(f"Pragmatic mode reduces barrier crossings vs emergency: {emergency_crossings} -> {pragmatic_crossings}") - if pragmatic_crossings > 0 and strict_crossings == 0: - print(f"Strict mode completely avoids private land (pragmatic crosses {pragmatic_crossings} cells)") - - # Time/distance comparison - if results["strict"]["total_time_minutes"] > results["emergency"]["total_time_minutes"]: - time_penalty = results["strict"]["total_time_minutes"] - results["emergency"]["total_time_minutes"] - print(f"Time cost of strict boundary respect: +{time_penalty:.1f} min") + print(f"Both paths have similar on-trail percentage") else: - print(f"Only {success_count}/3 modes found a path:") - for mode, result in results.items(): - if result["success"]: - print(f" {mode}: {result['total_distance_km']:.2f} km, {result['total_time_minutes']:.1f} min") - else: - print(f" {mode}: FAILED - {result.get('reason', 'unknown')}") + if not result_trails["success"]: + print(f"WITH TRAILS: FAILED - {result_trails.get('reason', 'unknown')}") + if not result_no_trails["success"]: + print(f"WITHOUT TRAILS: FAILED - {result_no_trails.get('reason', 'unknown')}") print(f"\n" + "-" * 80) print(f"Total wall time: {t_total - t0:.1f}s") - print(f"Closed cells in bbox: {closed_cells:,}") # Cleanup dem_reader.close() friction_reader.close() barrier_reader.close() + trail_reader.close() print("\nPrototype completed.") diff --git a/lib/offroute/trails.py b/lib/offroute/trails.py new file mode 100644 index 0000000..9d9185e --- /dev/null +++ b/lib/offroute/trails.py @@ -0,0 +1,174 @@ +""" +Trail corridor reader for OFFROUTE. + +Provides access to the OSM-derived trail raster for pathfinding. +Trail values replace WorldCover friction where trails exist. + +Raster values: + 0 = no trail (use WorldCover friction) + 5 = road (0.1× friction) + 15 = track (0.3× friction) + 25 = foot trail (0.5× friction) +""" +import numpy as np +from pathlib import Path +from typing import Tuple, Optional + +try: + import rasterio + from rasterio.windows import from_bounds + from rasterio.enums import Resampling +except ImportError: + raise ImportError("rasterio is required for trails layer support") + +# Default path to the trails raster +DEFAULT_TRAILS_PATH = Path("/mnt/nav/worldcover/trails.tif") + +# Trail value to friction multiplier mapping +TRAIL_FRICTION_MAP = { + 5: 0.1, # road + 15: 0.3, # track + 25: 0.5, # foot trail +} + + +class TrailReader: + """Reader for OSM-derived trail corridor raster.""" + + def __init__(self, trails_path: Path = DEFAULT_TRAILS_PATH): + self.trails_path = trails_path + self._dataset = None + + def _open(self): + """Lazy open the dataset.""" + if self._dataset is None: + if not self.trails_path.exists(): + raise FileNotFoundError( + f"Trails raster not found at {self.trails_path}. " + f"Run the Phase B rasterization script first." + ) + self._dataset = rasterio.open(self.trails_path) + return self._dataset + + def get_trails_grid( + self, + south: float, + north: float, + west: float, + east: float, + target_shape: Tuple[int, int] + ) -> np.ndarray: + """ + Get trail values for a bounding box, resampled to target shape. + + Args: + south, north, west, east: Bounding box coordinates (WGS84) + target_shape: (rows, cols) to resample to (matches elevation grid) + + Returns: + np.ndarray of uint8 trail values: + 0 = no trail + 5 = road (0.1× friction) + 15 = track (0.3× friction) + 25 = foot trail (0.5× friction) + """ + ds = self._open() + + # Create a window from the bounding box + window = from_bounds(west, south, east, north, ds.transform) + + # Read with resampling to target shape + # Use nearest neighbor to preserve discrete values + trails = ds.read( + 1, + window=window, + out_shape=target_shape, + resampling=Resampling.nearest + ) + + return trails + + def sample_point(self, lat: float, lon: float) -> int: + """Sample trail value at a single point.""" + ds = self._open() + + # Get pixel coordinates + row, col = ds.index(lon, lat) + + # Check bounds + if row < 0 or row >= ds.height or col < 0 or col >= ds.width: + return 0 # Out of bounds = no trail + + # Read single pixel + window = rasterio.windows.Window(col, row, 1, 1) + value = ds.read(1, window=window) + return int(value[0, 0]) + + def close(self): + """Close the dataset.""" + if self._dataset is not None: + self._dataset.close() + self._dataset = None + + +def trails_to_friction(trails: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + Convert trail values to friction multipliers. + + Args: + trails: uint8 array of trail values (0, 5, 15, or 25) + + Returns: + Tuple of: + - friction: float32 array of friction multipliers + - has_trail: bool array indicating where trails exist + """ + friction = np.ones_like(trails, dtype=np.float32) + has_trail = trails > 0 + + # Apply friction values where trails exist + friction[trails == 5] = 0.1 # road + friction[trails == 15] = 0.3 # track + friction[trails == 25] = 0.5 # foot trail + + return friction, has_trail + + +if __name__ == "__main__": + print("Testing TrailReader...") + + if not DEFAULT_TRAILS_PATH.exists(): + print(f"Trails raster not found at {DEFAULT_TRAILS_PATH}") + print("Run Phase B rasterization first.") + exit(1) + + reader = TrailReader() + + # Test point sampling - Twin Falls downtown (should have roads) + test_lat, test_lon = 42.563, -114.461 + trail_value = reader.sample_point(test_lat, test_lon) + print(f"\nTwin Falls ({test_lat}, {test_lon}): trail value = {trail_value}") + label = {0: "no trail", 5: "road", 15: "track", 25: "trail"}.get(trail_value, "unknown") + print(f" Type: {label}") + + # Test grid read for test bbox + trails = reader.get_trails_grid( + south=42.21, north=42.60, west=-114.76, east=-113.79, + target_shape=(400, 1000) + ) + print(f"\nGrid test shape: {trails.shape}") + + unique, counts = np.unique(trails, return_counts=True) + print("Value distribution:") + for v, c in zip(unique, counts): + pct = 100 * c / trails.size + label = {0: "no trail", 5: "road", 15: "track", 25: "trail"}.get(v, f"unknown({v})") + print(f" {label}: {c:,} pixels ({pct:.2f}%)") + + # Test conversion to friction + friction, has_trail = trails_to_friction(trails) + print(f"\nTrail coverage: {100 * np.sum(has_trail) / trails.size:.2f}%") + print(f"Friction range (on trails): {friction[has_trail].min():.1f} - {friction[has_trail].max():.1f}") + + reader.close() + print("\nTrailReader test complete.") From 1a9dfc8f8d78fb5c7a9798b58fa94cd6d648f6a8 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 13:44:34 +0000 Subject: [PATCH 05/16] =?UTF-8?q?feat(offroute):=20Phase=20O3b=20=E2=80=94?= =?UTF-8?q?=20trail=20entry=20index,=20Valhalla=20stitching,=20/api/offrou?= =?UTF-8?q?te=20endpoint?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Phase A: Trail Entry Point Index - Extract highway endpoints from idaho-latest.osm.pbf using osmium + ogr2ogr - Store 740,430 entry points in /mnt/nav/navi.db (SQLite with spatial index) - Entry points by class: service (271k), footway (152k), residential (146k), track (111k), path (26k), unclassified (16k), tertiary (9k), secondary (4k), primary (4k), bridleway (15) Phase B: Pathfinder → Valhalla Stitching (router.py) - OffrouteRouter orchestrates wilderness pathfinding + Valhalla on-network routing - Queries entry points within 50km (expanding to 100km if needed) - MCP pathfinder routes to nearest reachable entry point - Calls Valhalla pedestrian/bicycle/auto costing for on-network segment - Returns GeoJSON FeatureCollection with wilderness + network + combined segments Phase C: Flask Endpoint - POST /api/offroute with start/end coordinates, mode, boundary_mode - Returns GeoJSON route with per-segment metadata and turn-by-turn maneuvers Validated: 42.35,-114.30 → Twin Falls downtown - Wilderness: 0.5km, 9min | Network: 36km, 413min | Total: ~421min - 21 turn-by-turn instructions, segments connect at entry point Co-Authored-By: Claude Opus 4.5 --- lib/api.py | 77 +++++ lib/offroute/router.py | 752 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 829 insertions(+) create mode 100644 lib/offroute/router.py diff --git a/lib/api.py b/lib/api.py index 8a1f383..cd32e33 100644 --- a/lib/api.py +++ b/lib/api.py @@ -2722,3 +2722,80 @@ def api_auth_whoami(): 'authenticated': False, 'username': None, }) + + +# ── OFFROUTE API ── + +@app.route("/api/offroute", methods=["POST"]) +def api_offroute(): + """ + Off-network routing from wilderness to destination. + + Request body: + { + "start": [lat, lon], + "end": [lat, lon], + "mode": "foot" | "mtb" | "atv", (default: "foot") + "boundary_mode": "strict" | "pragmatic" | "emergency" (default: "pragmatic") + } + + Response: + { + "status": "ok", + "route": { GeoJSON FeatureCollection with wilderness + network segments }, + "summary": { total_distance_km, total_effort_minutes, ... } + } + """ + try: + data = request.get_json() + if not data: + return jsonify({"status": "error", "message": "No JSON body provided"}), 400 + + # Parse coordinates + start = data.get("start") + end = data.get("end") + + if not start or not end: + return jsonify({"status": "error", "message": "Missing start or end coordinates"}), 400 + + if not isinstance(start, (list, tuple)) or len(start) != 2: + return jsonify({"status": "error", "message": "start must be [lat, lon]"}), 400 + if not isinstance(end, (list, tuple)) or len(end) != 2: + return jsonify({"status": "error", "message": "end must be [lat, lon]"}), 400 + + start_lat, start_lon = float(start[0]), float(start[1]) + end_lat, end_lon = float(end[0]), float(end[1]) + + # Parse options + mode = data.get("mode", "foot") + if mode not in ("foot", "mtb", "atv"): + return jsonify({"status": "error", "message": "mode must be foot, mtb, or atv"}), 400 + + boundary_mode = data.get("boundary_mode", "pragmatic") + if boundary_mode not in ("strict", "pragmatic", "emergency"): + return jsonify({"status": "error", "message": "boundary_mode must be strict, pragmatic, or emergency"}), 400 + + # Import and run router + from .offroute.router import OffrouteRouter + + router = OffrouteRouter() + try: + result = router.route( + start_lat=start_lat, + start_lon=start_lon, + end_lat=end_lat, + end_lon=end_lon, + mode=mode, + boundary_mode=boundary_mode + ) + finally: + router.close() + + if result.get("status") == "error": + return jsonify(result), 400 + + return jsonify(result) + + except Exception as e: + logger.exception("Offroute error") + return jsonify({"status": "error", "message": str(e)}), 500 diff --git a/lib/offroute/router.py b/lib/offroute/router.py new file mode 100644 index 0000000..57d6ce5 --- /dev/null +++ b/lib/offroute/router.py @@ -0,0 +1,752 @@ +""" +OFFROUTE Router — Wilderness to network path orchestration. + +Connects the raster pathfinder (wilderness segment) to Valhalla (on-network segment). + +Entry points are extracted from OSM highways and stored in /mnt/nav/navi.db. +The pathfinder routes from a wilderness start to the nearest entry point, +then Valhalla completes the route to the destination. +""" +import json +import math +import sqlite3 +import subprocess +import tempfile +import time +from pathlib import Path +from typing import Dict, List, Optional, Tuple, Literal + +import numpy as np +import requests +from skimage.graph import MCP_Geometric + +from .dem import DEMReader +from .cost import compute_cost_grid +from .friction import FrictionReader, friction_to_multiplier +from .barriers import BarrierReader +from .trails import TrailReader + +# Paths +NAVI_DB_PATH = Path("/mnt/nav/navi.db") +OSM_PBF_PATH = Path("/mnt/nav/sources/idaho-latest.osm.pbf") + +# Valhalla endpoint +VALHALLA_URL = "http://localhost:8002" + +# Search radius for entry points (km) +DEFAULT_SEARCH_RADIUS_KM = 50 +EXPANDED_SEARCH_RADIUS_KM = 100 + +# Memory limit +MEMORY_LIMIT_GB = 12 + +# Mode to Valhalla costing mapping +MODE_TO_COSTING = { + "foot": "pedestrian", + "mtb": "bicycle", + "atv": "auto", +} + + +def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """Calculate distance between two points in meters.""" + R = 6371000 + dlat = math.radians(lat2 - lat1) + dlon = math.radians(lon2 - lon1) + a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2 + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + return R * c + + +def check_memory_usage() -> float: + """Check current memory usage in GB.""" + try: + import psutil + process = psutil.Process() + return process.memory_info().rss / (1024**3) + except ImportError: + return 0 + + +class EntryPointIndex: + """ + Trail entry point index for wilderness-to-network handoff. + + Entry points are endpoints and intersections of OSM highways + that connect wilderness areas to the routable network. + """ + + def __init__(self, db_path: Path = NAVI_DB_PATH): + self.db_path = db_path + self._conn = None + + def _get_conn(self) -> sqlite3.Connection: + if self._conn is None: + self._conn = sqlite3.connect(str(self.db_path)) + self._conn.row_factory = sqlite3.Row + return self._conn + + def table_exists(self) -> bool: + """Check if trail_entry_points table exists.""" + if not self.db_path.exists(): + return False + conn = self._get_conn() + cur = conn.execute( + "SELECT name FROM sqlite_master WHERE type='table' AND name='trail_entry_points'" + ) + return cur.fetchone() is not None + + def get_entry_point_count(self) -> int: + """Get count of entry points.""" + if not self.table_exists(): + return 0 + conn = self._get_conn() + cur = conn.execute("SELECT COUNT(*) FROM trail_entry_points") + return cur.fetchone()[0] + + def query_bbox(self, south: float, north: float, west: float, east: float) -> List[Dict]: + """Query entry points within a bounding box.""" + if not self.table_exists(): + return [] + + conn = self._get_conn() + cur = conn.execute(""" + SELECT id, lat, lon, highway_class, name + FROM trail_entry_points + WHERE lat >= ? AND lat <= ? AND lon >= ? AND lon <= ? + """, (south, north, west, east)) + + return [dict(row) for row in cur.fetchall()] + + def query_radius(self, lat: float, lon: float, radius_km: float) -> List[Dict]: + """Query entry points within radius of a point.""" + # Approximate bbox for the radius + lat_delta = radius_km / 111.0 + lon_delta = radius_km / (111.0 * math.cos(math.radians(lat))) + + points = self.query_bbox( + lat - lat_delta, lat + lat_delta, + lon - lon_delta, lon + lon_delta + ) + + # Filter by actual distance and add distance field + result = [] + for p in points: + dist = haversine_distance(lat, lon, p['lat'], p['lon']) + if dist <= radius_km * 1000: + p['distance_m'] = dist + result.append(p) + + return sorted(result, key=lambda x: x['distance_m']) + + def build_index(self, osm_pbf_path: Path = OSM_PBF_PATH) -> Dict: + """ + Build the entry point index from OSM PBF. + + Extracts endpoints of highway features that connect to the network. + """ + if not osm_pbf_path.exists(): + raise FileNotFoundError(f"OSM PBF not found: {osm_pbf_path}") + + print(f"Building trail entry point index from {osm_pbf_path}...") + + # Highway types to extract (routable network entry points) + highway_types = [ + "primary", "secondary", "tertiary", "unclassified", + "residential", "service", "track", "path", "footway", "bridleway" + ] + + stats = {"total": 0, "by_class": {}} + + with tempfile.TemporaryDirectory() as tmpdir: + # Extract highways to GeoJSON + geojson_path = Path(tmpdir) / "highways.geojson" + + # Build osmium tags-filter expressions (one per highway type) + print(f" Extracting highways with osmium...") + cmd = [ + "osmium", "tags-filter", + str(osm_pbf_path), + ] + # Add each highway type as a separate filter expression + for ht in highway_types: + cmd.append(f"w/highway={ht}") + cmd.extend(["-o", str(Path(tmpdir) / "filtered.osm.pbf"), "--overwrite"]) + + subprocess.run(cmd, check=True, capture_output=True) + + # Convert to GeoJSON + print(f" Converting to GeoJSON with ogr2ogr...") + cmd = [ + "ogr2ogr", "-f", "GeoJSON", + str(geojson_path), + str(Path(tmpdir) / "filtered.osm.pbf"), + "lines", + "-t_srs", "EPSG:4326" + ] + subprocess.run(cmd, check=True, capture_output=True) + + # Parse GeoJSON and extract endpoints + print(f" Extracting entry points...") + with open(geojson_path) as f: + data = json.load(f) + + # Collect unique points (endpoints) + # Key: (lat, lon) rounded to 5 decimal places (~1m precision) + points = {} + + for feature in data.get("features", []): + props = feature.get("properties", {}) + geom = feature.get("geometry", {}) + + if geom.get("type") != "LineString": + continue + + coords = geom.get("coordinates", []) + if len(coords) < 2: + continue + + highway_class = props.get("highway", "unknown") + name = props.get("name", "") + + # Extract endpoints + for coord in [coords[0], coords[-1]]: + lon, lat = coord[0], coord[1] + key = (round(lat, 5), round(lon, 5)) + + if key not in points: + points[key] = { + "lat": lat, + "lon": lon, + "highway_class": highway_class, + "name": name + } + else: + # Keep the "best" highway class (roads > tracks > paths) + existing = points[key] + if self._highway_priority(highway_class) < self._highway_priority(existing["highway_class"]): + points[key]["highway_class"] = highway_class + if name and not existing["name"]: + points[key]["name"] = name + + # Create/update database + print(f" Writing {len(points)} entry points to {self.db_path}...") + + self.db_path.parent.mkdir(parents=True, exist_ok=True) + conn = self._get_conn() + + # Create table + conn.execute(""" + CREATE TABLE IF NOT EXISTS trail_entry_points ( + id INTEGER PRIMARY KEY AUTOINCREMENT, + lat REAL NOT NULL, + lon REAL NOT NULL, + highway_class TEXT NOT NULL, + name TEXT + ) + """) + + # Clear existing data + conn.execute("DELETE FROM trail_entry_points") + + # Insert new points + for point in points.values(): + conn.execute(""" + INSERT INTO trail_entry_points (lat, lon, highway_class, name) + VALUES (?, ?, ?, ?) + """, (point["lat"], point["lon"], point["highway_class"], point["name"])) + + stats["total"] += 1 + hc = point["highway_class"] + stats["by_class"][hc] = stats["by_class"].get(hc, 0) + 1 + + # Create spatial index + conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lat ON trail_entry_points(lat)") + conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lon ON trail_entry_points(lon)") + conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_latlon ON trail_entry_points(lat, lon)") + + conn.commit() + + print(f" Done. Total: {stats['total']} entry points") + for hc, count in sorted(stats["by_class"].items(), key=lambda x: -x[1]): + print(f" {hc}: {count}") + + return stats + + def _highway_priority(self, highway_class: str) -> int: + """Lower number = better priority for entry points.""" + priority = { + "primary": 1, "secondary": 2, "tertiary": 3, + "unclassified": 4, "residential": 5, "service": 6, + "track": 7, "path": 8, "footway": 9, "bridleway": 10 + } + return priority.get(highway_class, 99) + + def close(self): + if self._conn: + self._conn.close() + self._conn = None + + +class OffrouteRouter: + """ + OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. + """ + + def __init__(self): + self.dem_reader = None + self.friction_reader = None + self.barrier_reader = None + self.trail_reader = None + self.entry_index = EntryPointIndex() + + def _init_readers(self): + """Lazy init readers.""" + if self.dem_reader is None: + self.dem_reader = DEMReader() + if self.friction_reader is None: + self.friction_reader = FrictionReader() + if self.barrier_reader is None: + self.barrier_reader = BarrierReader() + if self.trail_reader is None: + self.trail_reader = TrailReader() + + def route( + self, + start_lat: float, + start_lon: float, + end_lat: float, + end_lon: float, + mode: Literal["foot", "mtb", "atv"] = "foot", + boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" + ) -> Dict: + """ + Route from a wilderness start point to a destination. + + Returns a GeoJSON FeatureCollection with wilderness and network segments. + """ + t0 = time.time() + + # Ensure entry point index exists + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + # Find entry points near start + entry_points = self.entry_index.query_radius( + start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM + ) + + if not entry_points: + # Try expanded radius + entry_points = self.entry_index.query_radius( + start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM + ) + if not entry_points: + return { + "status": "error", + "message": f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start" + } + + # Build bbox for pathfinding grid + # Include start, end, and all entry points + all_lats = [start_lat, end_lat] + [p["lat"] for p in entry_points] + all_lons = [start_lon, end_lon] + [p["lon"] for p in entry_points] + + padding = 0.05 # ~5km padding + bbox = { + "south": min(all_lats) - padding, + "north": max(all_lats) + padding, + "west": min(all_lons) - padding, + "east": max(all_lons) + padding, + } + + # Initialize readers + self._init_readers() + + # Load elevation + try: + elevation, meta = self.dem_reader.get_elevation_grid( + south=bbox["south"], + north=bbox["north"], + west=bbox["west"], + east=bbox["east"], + ) + except Exception as e: + return {"status": "error", "message": f"Failed to load elevation: {e}"} + + # Check memory + mem = check_memory_usage() + if mem > MEMORY_LIMIT_GB: + return {"status": "error", "message": f"Memory limit exceeded: {mem:.1f}GB > {MEMORY_LIMIT_GB}GB"} + + # Load friction + friction_raw = self.friction_reader.get_friction_grid( + south=bbox["south"], + north=bbox["north"], + west=bbox["west"], + east=bbox["east"], + target_shape=elevation.shape + ) + friction_mult = friction_to_multiplier(friction_raw) + + # Load barriers + barriers = self.barrier_reader.get_barrier_grid( + south=bbox["south"], + north=bbox["north"], + west=bbox["west"], + east=bbox["east"], + target_shape=elevation.shape + ) + + # Load trails + trails = self.trail_reader.get_trails_grid( + south=bbox["south"], + north=bbox["north"], + west=bbox["west"], + east=bbox["east"], + target_shape=elevation.shape + ) + + # Compute cost grid + cost = compute_cost_grid( + elevation, + cell_size_m=meta["cell_size_m"], + friction=friction_mult, + trails=trails, + barriers=barriers, + boundary_mode=boundary_mode, + ) + + # Convert start to pixel coordinates + start_row, start_col = self.dem_reader.latlon_to_pixel(start_lat, start_lon, meta) + + # Validate start is in bounds + rows, cols = elevation.shape + if not (0 <= start_row < rows and 0 <= start_col < cols): + return {"status": "error", "message": "Start point outside grid bounds"} + + # Mark entry points on the grid + entry_pixels = [] + for ep in entry_points: + row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) + if 0 <= row < rows and 0 <= col < cols: + entry_pixels.append({ + "row": row, + "col": col, + "entry_point": ep + }) + + if not entry_pixels: + return {"status": "error", "message": "No entry points map to grid bounds"} + + # Run MCP pathfinder + mcp = MCP_Geometric(cost, fully_connected=True) + cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) + + # Find nearest reachable entry point + best_entry = None + best_cost = np.inf + + 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): + return { + "status": "error", + "message": "No path found to any entry point (blocked by impassable terrain)" + } + + # Traceback wilderness path + path_indices = mcp.traceback((best_entry["row"], best_entry["col"])) + + # Convert to coordinates and collect stats + wilderness_coords = [] + elevations = [] + trail_values = [] + + for row, col in path_indices: + lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) + wilderness_coords.append([lon, lat]) + elevations.append(elevation[row, col]) + trail_values.append(trails[row, col]) + + # Calculate wilderness segment stats + wilderness_distance_m = 0 + for i in range(1, len(wilderness_coords)): + lon1, lat1 = wilderness_coords[i-1] + lon2, lat2 = wilderness_coords[i] + wilderness_distance_m += haversine_distance(lat1, lon1, lat2, lon2) + + elev_arr = np.array(elevations) + elev_diff = np.diff(elev_arr) + wilderness_gain = float(np.sum(elev_diff[elev_diff > 0])) + wilderness_loss = float(np.sum(np.abs(elev_diff[elev_diff < 0]))) + + trail_arr = np.array(trail_values) + on_trail_cells = np.sum(trail_arr > 0) + total_cells = len(trail_arr) + on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 + + # Entry point reached + entry_lat = best_entry["entry_point"]["lat"] + entry_lon = best_entry["entry_point"]["lon"] + entry_class = best_entry["entry_point"]["highway_class"] + entry_name = best_entry["entry_point"].get("name", "") + + # Call Valhalla for on-network segment + valhalla_costing = MODE_TO_COSTING.get(mode, "pedestrian") + + valhalla_request = { + "locations": [ + {"lat": entry_lat, "lon": entry_lon}, + {"lat": end_lat, "lon": end_lon} + ], + "costing": valhalla_costing, + "directions_options": {"units": "kilometers"} + } + + network_segment = None + valhalla_error = None + + try: + resp = requests.post( + f"{VALHALLA_URL}/route", + json=valhalla_request, + timeout=30 + ) + + if resp.status_code == 200: + valhalla_data = resp.json() + trip = valhalla_data.get("trip", {}) + legs = trip.get("legs", []) + + if legs: + leg = legs[0] + shape = leg.get("shape", "") + + # Decode polyline6 + network_coords = self._decode_polyline(shape) + + # Extract maneuvers + maneuvers = [] + for m in leg.get("maneuvers", []): + maneuvers.append({ + "instruction": m.get("instruction", ""), + "type": m.get("type", 0), + "distance_km": m.get("length", 0), + "time_seconds": m.get("time", 0), + "street_names": m.get("street_names", []), + }) + + summary = trip.get("summary", {}) + network_segment = { + "coordinates": network_coords, + "distance_km": summary.get("length", 0), + "duration_minutes": summary.get("time", 0) / 60, + "maneuvers": maneuvers, + } + else: + valhalla_error = f"Valhalla returned {resp.status_code}: {resp.text[:200]}" + + except Exception as e: + valhalla_error = f"Valhalla request failed: {e}" + + # Build response + features = [] + + # Feature 1: Wilderness segment + wilderness_feature = { + "type": "Feature", + "properties": { + "segment_type": "wilderness", + "effort_minutes": float(best_cost / 60), + "distance_km": float(wilderness_distance_m / 1000), + "elevation_gain_m": wilderness_gain, + "elevation_loss_m": wilderness_loss, + "boundary_mode": boundary_mode, + "on_trail_pct": on_trail_pct, + "cell_count": total_cells, + }, + "geometry": { + "type": "LineString", + "coordinates": wilderness_coords, + } + } + features.append(wilderness_feature) + + # Feature 2: Network segment (if available) + if network_segment: + network_feature = { + "type": "Feature", + "properties": { + "segment_type": "network", + "distance_km": network_segment["distance_km"], + "duration_minutes": network_segment["duration_minutes"], + "maneuvers": network_segment["maneuvers"], + }, + "geometry": { + "type": "LineString", + "coordinates": network_segment["coordinates"], + } + } + features.append(network_feature) + + # Build combined route coordinates + combined_coords = wilderness_coords.copy() + if network_segment: + # Skip first point of network segment (it's the same as last wilderness point) + combined_coords.extend(network_segment["coordinates"][1:]) + + # Feature 3: Combined route + combined_feature = { + "type": "Feature", + "properties": { + "segment_type": "combined", + "mode": mode, + "boundary_mode": boundary_mode, + }, + "geometry": { + "type": "LineString", + "coordinates": combined_coords, + } + } + features.append(combined_feature) + + geojson = { + "type": "FeatureCollection", + "features": features, + } + + # Build summary + total_distance_km = wilderness_distance_m / 1000 + total_effort_minutes = best_cost / 60 + + if network_segment: + total_distance_km += network_segment["distance_km"] + total_effort_minutes += network_segment["duration_minutes"] + + summary = { + "total_distance_km": float(total_distance_km), + "total_effort_minutes": float(total_effort_minutes), + "wilderness_distance_km": float(wilderness_distance_m / 1000), + "wilderness_effort_minutes": float(best_cost / 60), + "network_distance_km": float(network_segment["distance_km"]) if network_segment else 0, + "network_duration_minutes": float(network_segment["duration_minutes"]) if network_segment else 0, + "on_trail_pct": on_trail_pct, + "boundary_mode": boundary_mode, + "mode": mode, + "entry_point": { + "lat": entry_lat, + "lon": entry_lon, + "highway_class": entry_class, + "name": entry_name, + }, + "computation_time_s": time.time() - t0, + } + + result = { + "status": "ok", + "route": geojson, + "summary": summary, + } + + if valhalla_error: + result["warning"] = f"Network segment incomplete: {valhalla_error}" + + return result + + def _decode_polyline(self, encoded: str, precision: int = 6) -> List[List[float]]: + """Decode a polyline string into coordinates [lon, lat].""" + coords = [] + index = 0 + lat = 0 + lon = 0 + + while index < len(encoded): + # Latitude + shift = 0 + result = 0 + while True: + b = ord(encoded[index]) - 63 + index += 1 + result |= (b & 0x1f) << shift + shift += 5 + if b < 0x20: + break + dlat = ~(result >> 1) if result & 1 else result >> 1 + lat += dlat + + # Longitude + shift = 0 + result = 0 + while True: + b = ord(encoded[index]) - 63 + index += 1 + result |= (b & 0x1f) << shift + shift += 5 + if b < 0x20: + break + dlon = ~(result >> 1) if result & 1 else result >> 1 + lon += dlon + + coords.append([lon / (10 ** precision), lat / (10 ** precision)]) + + return coords + + def close(self): + """Close all readers.""" + if self.dem_reader: + self.dem_reader.close() + if self.friction_reader: + self.friction_reader.close() + if self.barrier_reader: + self.barrier_reader.close() + if self.trail_reader: + self.trail_reader.close() + self.entry_index.close() + + +def build_entry_index(): + """Build the trail entry point index.""" + index = EntryPointIndex() + stats = index.build_index() + index.close() + return stats + + +if __name__ == "__main__": + import sys + + if len(sys.argv) > 1 and sys.argv[1] == "build": + print("Building trail entry point index...") + stats = build_entry_index() + print(f"\nDone. Total entry points: {stats['total']}") + + elif len(sys.argv) > 1 and sys.argv[1] == "test": + print("Testing router...") + + router = OffrouteRouter() + + # Test route: wilderness to Twin Falls + result = router.route( + start_lat=42.35, + start_lon=-114.30, + end_lat=42.5629, + end_lon=-114.4609, + mode="foot", + boundary_mode="pragmatic" + ) + + print(json.dumps(result, indent=2, default=str)) + router.close() + + else: + print("Usage:") + print(" python router.py build # Build entry point index") + print(" python router.py test # Test route") From bc463188d56b1cf9b3e804427ab5e0117743fc57 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 14:11:56 +0000 Subject: [PATCH 06/16] =?UTF-8?q?feat(offroute):=20Phase=20O4=20=E2=80=94?= =?UTF-8?q?=20multi-mode=20cost=20functions=20(foot/mtb/atv/vehicle)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add ModeProfile dataclass for data-driven mode configuration - Implement three speed functions: * Tobler off-path hiking (foot) * Herzog wheeled-transport polynomial (mtb/atv) * Linear speed degradation (vehicle) - Add WildernessReader for PAD-US Des_Tp=WA wilderness areas - Mode-specific terrain friction overrides: * Forest impassable for ATV/vehicle, high friction for MTB * Wetland/mangrove impassable for all wheeled modes - Trail access rules: * Foot trails (value 25) impassable for ATV/vehicle - Wilderness blocking for mtb/atv/vehicle modes - Vehicle mode allows flat grassland/cropland traversal - Memory optimization: limit entry points, constrain bbox size - Update router to pass mode and wilderness to cost function - Add vehicle to API mode validation Validated all four modes with test route: - foot: 0.46km off-network, 12.11km network, 89% on trail - mtb: 0.47km off-network, 13.13km network, 90% on trail - atv: 0.47km off-network, 12.81km network, 90% on trail - vehicle: 0.46km off-network, 12.81km network, 89% on trail Co-Authored-By: Claude Opus 4.5 --- lib/api.py | 4 +- lib/offroute/barriers.py | 274 +++++++++++++++++---- lib/offroute/cost.py | 515 +++++++++++++++++++++++++++++---------- lib/offroute/router.py | 274 ++++++++++----------- 4 files changed, 740 insertions(+), 327 deletions(-) diff --git a/lib/api.py b/lib/api.py index cd32e33..a127866 100644 --- a/lib/api.py +++ b/lib/api.py @@ -2768,8 +2768,8 @@ def api_offroute(): # Parse options mode = data.get("mode", "foot") - if mode not in ("foot", "mtb", "atv"): - return jsonify({"status": "error", "message": "mode must be foot, mtb, or atv"}), 400 + if mode not in ("foot", "mtb", "atv", "vehicle"): + return jsonify({"status": "error", "message": "mode must be foot, mtb, atv, or vehicle"}), 400 boundary_mode = data.get("boundary_mode", "pragmatic") if boundary_mode not in ("strict", "pragmatic", "emergency"): diff --git a/lib/offroute/barriers.py b/lib/offroute/barriers.py index 7fcad75..f68e892 100644 --- a/lib/offroute/barriers.py +++ b/lib/offroute/barriers.py @@ -1,11 +1,12 @@ """ -PAD-US barrier layer for OFFROUTE. +PAD-US barrier and wilderness layers for OFFROUTE. -Provides access to the PAD-US land ownership raster for routing decisions. -Cells with value 255 represent closed/restricted areas (Pub_Access = XA). +Provides access to: +1. Barrier raster (Pub_Access = 'XA' - closed/restricted areas) +2. Wilderness raster (Des_Tp = 'WA' - designated wilderness areas) -Build function rasterizes PAD-US geodatabase to aligned GeoTIFF. -Runtime functions read the raster and resample to match elevation grids. +Build functions rasterize PAD-US geodatabase to aligned GeoTIFFs. +Runtime functions read the rasters and resample to match elevation grids. """ import numpy as np from pathlib import Path @@ -23,6 +24,7 @@ except ImportError: # Paths DEFAULT_BARRIERS_PATH = Path("/mnt/nav/worldcover/padus_barriers.tif") +DEFAULT_WILDERNESS_PATH = Path("/mnt/nav/worldcover/wilderness.tif") PADUS_GDB_PATH = Path("/mnt/nav/padus/PADUS4_0_Geodatabase.gdb") PADUS_LAYER = "PADUS4_0Combined_Proclamation_Marine_Fee_Designation_Easement" @@ -39,7 +41,7 @@ PIXEL_SIZE = 0.0003 # ~33m class BarrierReader: - """Reader for PAD-US barrier raster.""" + """Reader for PAD-US barrier raster (closed/restricted areas).""" def __init__(self, barrier_path: Path = DEFAULT_BARRIERS_PATH): self.barrier_path = barrier_path @@ -77,32 +79,86 @@ class BarrierReader: 0 = public/accessible """ ds = self._open() - - # Create a window from the bounding box window = from_bounds(west, south, east, north, ds.transform) - - # Read with resampling to target shape barriers = ds.read( 1, window=window, out_shape=target_shape, resampling=Resampling.nearest ) - return barriers def sample_point(self, lat: float, lon: float) -> int: """Sample barrier value at a single point.""" ds = self._open() - - # Get pixel coordinates row, col = ds.index(lon, lat) - - # Check bounds if row < 0 or row >= ds.height or col < 0 or col >= ds.width: - return 0 # Out of bounds = accessible + return 0 + window = rasterio.windows.Window(col, row, 1, 1) + value = ds.read(1, window=window) + return int(value[0, 0]) - # Read single pixel + def close(self): + """Close the dataset.""" + if self._dataset is not None: + self._dataset.close() + self._dataset = None + + +class WildernessReader: + """Reader for PAD-US wilderness raster (designated wilderness areas).""" + + def __init__(self, wilderness_path: Path = DEFAULT_WILDERNESS_PATH): + self.wilderness_path = wilderness_path + self._dataset = None + + def _open(self): + """Lazy open the dataset.""" + if self._dataset is None: + if not self.wilderness_path.exists(): + raise FileNotFoundError( + f"Wilderness raster not found at {self.wilderness_path}. " + f"Run build_wilderness_raster() first." + ) + self._dataset = rasterio.open(self.wilderness_path) + return self._dataset + + def get_wilderness_grid( + self, + south: float, + north: float, + west: float, + east: float, + target_shape: Tuple[int, int] + ) -> np.ndarray: + """ + Get wilderness values for a bounding box, resampled to target shape. + + Args: + south, north, west, east: Bounding box coordinates (WGS84) + target_shape: (rows, cols) to resample to (matches elevation grid) + + Returns: + np.ndarray of uint8 wilderness values: + 255 = designated wilderness area + 0 = not wilderness + """ + ds = self._open() + window = from_bounds(west, south, east, north, ds.transform) + wilderness = ds.read( + 1, + window=window, + out_shape=target_shape, + resampling=Resampling.nearest + ) + return wilderness + + def sample_point(self, lat: float, lon: float) -> int: + """Sample wilderness value at a single point.""" + ds = self._open() + row, col = ds.index(lon, lat) + if row < 0 or row >= ds.height or col < 0 or col >= ds.width: + return 0 window = rasterio.windows.Window(col, row, 1, 1) value = ds.read(1, window=window) return int(value[0, 0]) @@ -124,22 +180,12 @@ def build_barriers_raster( Build the PAD-US barriers raster from the source geodatabase. Extracts polygons where Pub_Access = 'XA' (Closed) and rasterizes them. - - Args: - output_path: Output GeoTIFF path - gdb_path: Path to PAD-US geodatabase - pixel_size: Pixel size in degrees - bounds: CONUS bounding box - - Returns: - Path to the created raster """ import shutil if not gdb_path.exists(): raise FileNotFoundError(f"PAD-US geodatabase not found at {gdb_path}") - # Check for required tools if not shutil.which('ogr2ogr'): raise RuntimeError("ogr2ogr not found. Install GDAL.") if not shutil.which('gdal_rasterize'): @@ -154,7 +200,6 @@ def build_barriers_raster( print(f" Bounds: {bounds}") with tempfile.TemporaryDirectory() as tmpdir: - # Step 1: Extract closed areas and reproject to WGS84 closed_gpkg = Path(tmpdir) / "closed_areas.gpkg" print(f"\n[1/3] Extracting closed areas (Pub_Access = 'XA')...") @@ -176,28 +221,23 @@ def build_barriers_raster( print(f"STDERR: {result.stderr}") raise RuntimeError(f"ogr2ogr failed: {result.stderr}") - # Check feature count info_cmd = ["ogrinfo", "-so", str(closed_gpkg), "closed_areas"] info_result = subprocess.run(info_cmd, capture_output=True, text=True) print(f" Extraction result:\n{info_result.stdout}") - # Step 2: Create empty raster print(f"\n[2/3] Creating raster grid...") width = int((bounds['east'] - bounds['west']) / pixel_size) height = int((bounds['north'] - bounds['south']) / pixel_size) - print(f" Grid size: {width} x {height} pixels") - print(f" Memory estimate: {width * height / 1e6:.1f} MB") - # Step 3: Rasterize print(f"\n[3/3] Rasterizing closed areas...") rasterize_cmd = [ "gdal_rasterize", "-burn", "255", "-init", "0", - "-a_nodata", "0", # No nodata - 0 means accessible + "-a_nodata", "0", "-te", str(bounds['west']), str(bounds['south']), str(bounds['east']), str(bounds['north']), "-tr", str(pixel_size), str(pixel_size), @@ -214,14 +254,10 @@ def build_barriers_raster( print(f"STDERR: {result.stderr}") raise RuntimeError(f"gdal_rasterize failed: {result.stderr}") - # Verify output print(f"\n[Done] Verifying output...") with rasterio.open(output_path) as ds: print(f" Size: {ds.width} x {ds.height}") print(f" CRS: {ds.crs}") - print(f" Bounds: {ds.bounds}") - - # Sample a few tiles to check sample = ds.read(1, window=rasterio.windows.Window(0, 0, 1000, 1000)) closed_count = np.sum(sample == 255) print(f" Sample (1000x1000): {closed_count} closed cells") @@ -232,17 +268,140 @@ def build_barriers_raster( return output_path +def build_wilderness_raster( + output_path: Path = DEFAULT_WILDERNESS_PATH, + gdb_path: Path = PADUS_GDB_PATH, + pixel_size: float = PIXEL_SIZE, + bounds: dict = CONUS_BOUNDS, +) -> Path: + """ + Build the PAD-US wilderness raster from the source geodatabase. + + Extracts polygons where Des_Tp = 'WA' (Wilderness Area) and rasterizes them. + """ + import shutil + + if not gdb_path.exists(): + raise FileNotFoundError(f"PAD-US geodatabase not found at {gdb_path}") + + if not shutil.which('ogr2ogr'): + raise RuntimeError("ogr2ogr not found. Install GDAL.") + if not shutil.which('gdal_rasterize'): + raise RuntimeError("gdal_rasterize not found. Install GDAL.") + + output_path.parent.mkdir(parents=True, exist_ok=True) + + print(f"Building PAD-US wilderness raster...") + print(f" Source: {gdb_path}") + print(f" Output: {output_path}") + print(f" Pixel size: {pixel_size} degrees (~{pixel_size * 111000:.0f}m)") + print(f" Bounds: {bounds}") + + with tempfile.TemporaryDirectory() as tmpdir: + wilderness_gpkg = Path(tmpdir) / "wilderness_areas.gpkg" + + print(f"\n[1/3] Extracting wilderness areas (Des_Tp = 'WA')...") + + ogr_cmd = [ + "ogr2ogr", + "-f", "GPKG", + str(wilderness_gpkg), + str(gdb_path), + PADUS_LAYER, + "-where", "Des_Tp = 'WA'", + "-t_srs", "EPSG:4326", + "-nlt", "MULTIPOLYGON", + "-nln", "wilderness_areas", + ] + + result = subprocess.run(ogr_cmd, capture_output=True, text=True) + if result.returncode != 0: + print(f"STDERR: {result.stderr}") + raise RuntimeError(f"ogr2ogr failed: {result.stderr}") + + info_cmd = ["ogrinfo", "-so", str(wilderness_gpkg), "wilderness_areas"] + info_result = subprocess.run(info_cmd, capture_output=True, text=True) + print(f" Extraction result:\n{info_result.stdout}") + + print(f"\n[2/3] Creating raster grid...") + + width = int((bounds['east'] - bounds['west']) / pixel_size) + height = int((bounds['north'] - bounds['south']) / pixel_size) + print(f" Grid size: {width} x {height} pixels") + + print(f"\n[3/3] Rasterizing wilderness areas...") + + rasterize_cmd = [ + "gdal_rasterize", + "-burn", "255", + "-init", "0", + "-a_nodata", "0", + "-te", str(bounds['west']), str(bounds['south']), + str(bounds['east']), str(bounds['north']), + "-tr", str(pixel_size), str(pixel_size), + "-ot", "Byte", + "-co", "COMPRESS=LZW", + "-co", "TILED=YES", + "-l", "wilderness_areas", + str(wilderness_gpkg), + str(output_path), + ] + + result = subprocess.run(rasterize_cmd, capture_output=True, text=True) + if result.returncode != 0: + print(f"STDERR: {result.stderr}") + raise RuntimeError(f"gdal_rasterize failed: {result.stderr}") + + print(f"\n[Done] Verifying output...") + with rasterio.open(output_path) as ds: + print(f" Size: {ds.width} x {ds.height}") + print(f" CRS: {ds.crs}") + sample = ds.read(1, window=rasterio.windows.Window(0, 0, 1000, 1000)) + wilderness_count = np.sum(sample == 255) + print(f" Sample (1000x1000): {wilderness_count} wilderness cells") + + file_size = output_path.stat().st_size / (1024**2) + print(f" File size: {file_size:.1f} MB") + + return output_path + + if __name__ == "__main__": import sys - if len(sys.argv) > 1 and sys.argv[1] == "build": - # Build the raster - print("="*60) - print("PAD-US Barriers Raster Build") - print("="*60) - build_barriers_raster() + if len(sys.argv) > 1: + cmd = sys.argv[1] + + if cmd == "build": + print("=" * 60) + print("PAD-US Barriers Raster Build") + print("=" * 60) + build_barriers_raster() + + elif cmd == "build-wilderness": + print("=" * 60) + print("PAD-US Wilderness Raster Build") + print("=" * 60) + build_wilderness_raster() + + elif cmd == "build-all": + print("=" * 60) + print("Building all PAD-US rasters") + print("=" * 60) + build_barriers_raster() + print("\n") + build_wilderness_raster() + + else: + print(f"Unknown command: {cmd}") + print("Usage:") + print(" python barriers.py build # Build barriers raster") + print(" python barriers.py build-wilderness # Build wilderness raster") + print(" python barriers.py build-all # Build both rasters") + sys.exit(1) + else: - # Test the reader + # Test readers print("Testing BarrierReader...") if not DEFAULT_BARRIERS_PATH.exists(): @@ -251,16 +410,31 @@ if __name__ == "__main__": sys.exit(1) reader = BarrierReader() - - # Test grid read for Idaho area barriers = reader.get_barrier_grid( south=42.2, north=42.6, west=-114.8, east=-113.8, target_shape=(400, 1000) ) - print(f"\nGrid test shape: {barriers.shape}") + print(f"\nBarrier grid shape: {barriers.shape}") print(f"Unique values: {np.unique(barriers)}") closed_cells = np.sum(barriers == 255) print(f"Closed cells: {closed_cells} ({100*closed_cells/barriers.size:.2f}%)") - reader.close() - print("\nBarrierReader test complete.") + + print("\nTesting WildernessReader...") + + if not DEFAULT_WILDERNESS_PATH.exists(): + print(f"Wilderness raster not found at {DEFAULT_WILDERNESS_PATH}") + print(f"Run: python barriers.py build-wilderness") + else: + wilderness_reader = WildernessReader() + wilderness = wilderness_reader.get_wilderness_grid( + south=42.2, north=42.6, west=-114.8, east=-113.8, + target_shape=(400, 1000) + ) + print(f"Wilderness grid shape: {wilderness.shape}") + print(f"Unique values: {np.unique(wilderness)}") + wilderness_cells = np.sum(wilderness == 255) + print(f"Wilderness cells: {wilderness_cells} ({100*wilderness_cells/wilderness.size:.2f}%)") + wilderness_reader.close() + + print("\nDone.") diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py index 5f3618c..c3b6a5a 100644 --- a/lib/offroute/cost.py +++ b/lib/offroute/cost.py @@ -1,43 +1,207 @@ """ -Tobler off-path hiking cost function for OFFROUTE. +Multi-mode travel cost functions for OFFROUTE. -Computes travel time cost based on terrain slope using Tobler's -hiking function with off-trail penalty. Optionally applies friction -multipliers from land cover data, trail corridors, and barrier grids. +Supports four travel modes: foot, mtb, atv, vehicle. +Each mode has its own speed function, max slope, trail access rules, +and terrain friction overrides. + +Mode profiles are data-driven — adding a new mode means adding a profile entry. """ import math import numpy as np -from typing import Optional, Literal +from dataclasses import dataclass, field +from typing import Optional, Literal, Dict, Callable -# Maximum passable slope in degrees -MAX_SLOPE_DEG = 40.0 +# ═══════════════════════════════════════════════════════════════════════════════ +# SPEED FUNCTIONS +# ═══════════════════════════════════════════════════════════════════════════════ + +def tobler_off_path_speed(grade: np.ndarray, base_speed: float = 6.0) -> np.ndarray: + """ + Tobler off-path hiking function. + + W = 0.6 * base_speed * exp(-3.5 * |S + 0.05|) + + Peak ~3.6 km/h at grade = -0.05 (slight downhill). + The 0.6 multiplier is the off-trail penalty. + """ + return 0.6 * base_speed * np.exp(-3.5 * np.abs(grade + 0.05)) + + +def herzog_wheeled_speed(grade: np.ndarray, base_speed: float = 12.0) -> np.ndarray: + """ + Herzog wheeled-transport polynomial. + + Relative speed factor: + 1 / (1337.8·S^6 + 278.19·S^5 − 517.39·S^4 − 78.199·S^3 + 93.419·S^2 + 19.825·|S| + 1.64) + + Multiply by base_speed to get km/h. + """ + S = grade + S_abs = np.abs(S) + + # Herzog polynomial (returns relative speed factor 0-1) + denom = (1337.8 * S**6 + 278.19 * S**5 - 517.39 * S**4 + - 78.199 * S**3 + 93.419 * S**2 + 19.825 * S_abs + 1.64) + + # Avoid division by zero and negative speeds + denom = np.maximum(denom, 0.1) + rel_speed = 1.0 / denom + + # Clamp relative speed to reasonable bounds (0.05 to 1.5) + rel_speed = np.clip(rel_speed, 0.05, 1.5) + + return base_speed * rel_speed + + +def linear_degrade_speed(grade: np.ndarray, base_speed: float = 40.0, max_grade: float = 0.364) -> np.ndarray: + """ + Linear speed degradation with slope. + + speed = base_speed * max(0, 1 - |grade| / max_grade) + + max_grade = tan(20°) ≈ 0.364 for 20° max slope. + """ + speed = base_speed * np.maximum(0, 1.0 - np.abs(grade) / max_grade) + return np.maximum(speed, 0.1) # Minimum crawl speed + + +# ═══════════════════════════════════════════════════════════════════════════════ +# MODE PROFILES (Data-driven configuration) +# ═══════════════════════════════════════════════════════════════════════════════ + +@dataclass +class ModeProfile: + """Configuration for a travel mode.""" + + name: str + description: str + + # Speed function parameters + speed_function: str # "tobler", "herzog", "linear" + base_speed_kmh: float + max_slope_deg: float + + # Trail access: trail_value -> friction multiplier (None = impassable) + # Trail values: 5=road, 15=track, 25=foot trail + trail_friction: Dict[int, Optional[float]] = field(default_factory=dict) + + # Off-trail terrain friction overrides (by WorldCover class) + # These MULTIPLY the base WorldCover friction + # None = use default, np.inf = impassable + # WorldCover values: 10=tree, 20=shrub, 30=grass, 40=crop, 50=urban, + # 60=bare, 80=water, 90=wetland, 95=mangrove, 100=moss + terrain_friction_override: Dict[int, Optional[float]] = field(default_factory=dict) + + # Should wilderness areas be impassable? + wilderness_impassable: bool = False + + # For vehicle mode: can traverse off-trail flat terrain? + off_trail_flat_threshold_deg: float = 0.0 # 0 = no off-trail allowed + off_trail_flat_friction: float = np.inf # friction if allowed + + +# Define all mode profiles +MODE_PROFILES: Dict[str, ModeProfile] = { + "foot": ModeProfile( + name="foot", + description="Hiking on foot (Tobler off-path model)", + speed_function="tobler", + base_speed_kmh=6.0, + max_slope_deg=40.0, + trail_friction={ + 5: 0.1, # road + 15: 0.3, # track + 25: 0.5, # foot trail + }, + terrain_friction_override={ + # Use default WorldCover friction for foot mode + }, + wilderness_impassable=False, + ), + + "mtb": ModeProfile( + name="mtb", + description="Mountain bike / dirt bike (Herzog wheeled model)", + speed_function="herzog", + base_speed_kmh=12.0, + max_slope_deg=25.0, + trail_friction={ + 5: 0.1, # road + 15: 0.2, # track + 25: 0.5, # foot trail (rideable but slow) + }, + terrain_friction_override={ + 30: 2.0, # Grassland: rideable but slow + 20: 4.0, # Shrubland: barely rideable + 10: 8.0, # Tree cover/forest: effectively impassable + 60: 3.0, # Bare/rocky + 90: np.inf, # Wetland: impassable + 95: np.inf, # Mangrove: impassable + 80: np.inf, # Water: impassable + }, + wilderness_impassable=True, + ), + + "atv": ModeProfile( + name="atv", + description="ATV / side-by-side (Herzog wheeled model, higher base speed)", + speed_function="herzog", + base_speed_kmh=25.0, + max_slope_deg=30.0, + trail_friction={ + 5: 0.1, # road + 15: 0.3, # track + 25: None, # foot trail: impassable (too narrow) + }, + terrain_friction_override={ + 30: 1.5, # Grassland: passable + 20: 3.0, # Shrubland: rough + 10: np.inf, # Forest: impassable + 60: 2.0, # Bare/rocky + 90: np.inf, # Wetland: impassable + 95: np.inf, # Mangrove: impassable + 80: np.inf, # Water: impassable + }, + wilderness_impassable=True, + ), + + "vehicle": ModeProfile( + name="vehicle", + description="4x4 truck / jeep (linear speed degradation)", + speed_function="linear", + base_speed_kmh=40.0, + max_slope_deg=20.0, + trail_friction={ + 5: 0.1, # road + 15: 0.5, # track (rough but passable) + 25: None, # foot trail: impassable + }, + terrain_friction_override={ + # All off-trail terrain is impassable by default + 10: np.inf, # Forest + 20: np.inf, # Shrubland + 30: np.inf, # Grassland (except flat - see below) + 40: np.inf, # Cropland (except flat - see below) + 60: np.inf, # Bare + 90: np.inf, # Wetland + 95: np.inf, # Mangrove + 80: np.inf, # Water + }, + wilderness_impassable=True, + off_trail_flat_threshold_deg=5.0, # Can drive on flat fields + off_trail_flat_friction=5.0, # But very slow + ), +} -# Tobler off-path parameters -TOBLER_BASE_SPEED = 6.0 -TOBLER_OFF_TRAIL_MULT = 0.6 # Pragmatic mode friction multiplier for private land PRAGMATIC_BARRIER_MULTIPLIER = 5.0 -# Trail value to friction multiplier mapping -# Trail friction REPLACES land cover friction (a road through forest is still easy) -TRAIL_FRICTION_MAP = { - 5: 0.1, # road - 15: 0.3, # track - 25: 0.5, # foot trail -} - - -def tobler_speed(grade: float) -> float: - """ - Calculate hiking speed using Tobler's off-path function. - - speed_kmh = 0.6 * 6.0 * exp(-3.5 * |grade + 0.05|) - - Peak speed is ~3.6 km/h at grade = -0.05 (slight downhill). - """ - return TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * math.exp(-3.5 * abs(grade + 0.05)) +# ═══════════════════════════════════════════════════════════════════════════════ +# COST GRID COMPUTATION +# ═══════════════════════════════════════════════════════════════════════════════ def compute_cost_grid( elevation: np.ndarray, @@ -45,16 +209,16 @@ def compute_cost_grid( cell_size_lat_m: float = None, cell_size_lon_m: float = None, friction: Optional[np.ndarray] = None, + friction_raw: Optional[np.ndarray] = None, trails: Optional[np.ndarray] = None, barriers: Optional[np.ndarray] = None, - boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" + wilderness: Optional[np.ndarray] = None, + boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic", + mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot" ) -> np.ndarray: """ Compute isotropic travel cost grid from elevation data. - Each cell's cost represents the time (in seconds) to traverse that cell, - based on the average slope from neighboring cells. - Args: elevation: 2D array of elevation values in meters cell_size_m: Average cell size in meters @@ -63,30 +227,29 @@ def compute_cost_grid( friction: Optional 2D array of friction multipliers (WorldCover). Values should be float (1.0 = baseline, 2.0 = 2x slower). np.inf marks impassable cells. - If None, no friction is applied (backward compatible). + friction_raw: Optional 2D array of raw WorldCover class values (uint8). + Used for mode-specific terrain overrides. + Values: 10=tree, 20=shrub, 30=grass, etc. trails: Optional 2D array of trail values (uint8). - 0 = no trail (use friction) - 5 = road (0.1× friction, replaces WorldCover) - 15 = track (0.3× friction, replaces WorldCover) - 25 = foot trail (0.5× friction, replaces WorldCover) - Trail friction REPLACES land cover friction where trails exist. - If None, no trail burn-in is applied. + 0 = no trail, 5 = road, 15 = track, 25 = foot trail barriers: Optional 2D array of barrier values (uint8). - 255 = closed/restricted area (from PAD-US Pub_Access = XA). - 0 = accessible. - If None, no barriers are applied. - boundary_mode: How to handle private/restricted land barriers: - "strict" - cells with barrier=255 become impassable (np.inf) - "pragmatic" - cells with barrier=255 get 5.0x friction penalty - "emergency" - barriers are ignored entirely - Default: "pragmatic" + 255 = closed/restricted area (PAD-US Pub_Access = XA). + wilderness: Optional[np.ndarray] of wilderness values (uint8). + 255 = designated wilderness area. + boundary_mode: How to handle barriers ("strict", "pragmatic", "emergency") + mode: Travel mode ("foot", "mtb", "atv", "vehicle") Returns: 2D array of travel cost in seconds per cell. np.inf for impassable cells. """ if boundary_mode not in ("strict", "pragmatic", "emergency"): - raise ValueError(f"boundary_mode must be 'strict', 'pragmatic', or 'emergency', got '{boundary_mode}'") + raise ValueError(f"boundary_mode must be 'strict', 'pragmatic', or 'emergency'") + + if mode not in MODE_PROFILES: + raise ValueError(f"mode must be one of {list(MODE_PROFILES.keys())}") + + profile = MODE_PROFILES[mode] if cell_size_lat_m is None: cell_size_lat_m = cell_size_m @@ -95,120 +258,212 @@ def compute_cost_grid( rows, cols = elevation.shape - # Compute gradients in both directions - dy = np.zeros_like(elevation) - dx = np.zeros_like(elevation) + # ─── Compute gradients (in-place where possible) ───────────────────────── + # Use float32 to reduce memory footprint + grade = np.zeros(elevation.shape, dtype=np.float32) - # Central differences for interior, forward/backward at edges - dy[1:-1, :] = (elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m) - dy[0, :] = (elevation[0, :] - elevation[1, :]) / cell_size_lat_m - dy[-1, :] = (elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m + # Compute dy contribution to grade squared + dy_contrib = np.zeros(elevation.shape, dtype=np.float32) + dy_contrib[1:-1, :] = ((elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m)) ** 2 + dy_contrib[0, :] = ((elevation[0, :] - elevation[1, :]) / cell_size_lat_m) ** 2 + dy_contrib[-1, :] = ((elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m) ** 2 - dx[:, 1:-1] = (elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m) - dx[:, 0] = (elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m - dx[:, -1] = (elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m + # Compute dx contribution and add to dy_contrib in-place + dy_contrib[:, 1:-1] += ((elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m)) ** 2 + dy_contrib[:, 0] += ((elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m) ** 2 + dy_contrib[:, -1] += ((elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m) ** 2 - # Compute slope magnitude (grade = rise/run) - grade_magnitude = np.sqrt(dx**2 + dy**2) + # grade = sqrt(dx^2 + dy^2) + np.sqrt(dy_contrib, out=grade) + del dy_contrib # Free memory immediately - # Convert to slope angle in degrees - slope_deg = np.degrees(np.arctan(grade_magnitude)) + # ─── Compute speed based on mode ───────────────────────────────────────── + max_grade_val = np.tan(np.radians(profile.max_slope_deg)) - # Compute speed for each cell using Tobler function - speed_kmh = TOBLER_OFF_TRAIL_MULT * TOBLER_BASE_SPEED * np.exp(-3.5 * np.abs(grade_magnitude + 0.05)) + if profile.speed_function == "tobler": + speed_kmh = tobler_off_path_speed(grade, profile.base_speed_kmh) + elif profile.speed_function == "herzog": + speed_kmh = herzog_wheeled_speed(grade, profile.base_speed_kmh) + elif profile.speed_function == "linear": + speed_kmh = linear_degrade_speed(grade, profile.base_speed_kmh, max_grade_val) + else: + raise ValueError(f"Unknown speed function: {profile.speed_function}") - # Convert speed to time cost (seconds to traverse one cell) + # ─── Base cost (seconds per cell) ───────────────────────────────────────── avg_cell_size = (cell_size_lat_m + cell_size_lon_m) / 2 - cost = avg_cell_size * 3.6 / speed_kmh + cost = (avg_cell_size * 3.6) / speed_kmh + del speed_kmh - # Set impassable cells (slope > MAX_SLOPE_DEG) to infinity - cost[slope_deg > MAX_SLOPE_DEG] = np.inf + # ─── Max slope limit ────────────────────────────────────────────────────── + cost[grade > max_grade_val] = np.inf - # Handle NaN elevations (no data) + # ─── NaN elevations ────────────────────────────────────────────────────── cost[np.isnan(elevation)] = np.inf - # Build effective friction array - # Start with WorldCover friction if provided, else 1.0 + # ─── Apply friction in-place ───────────────────────────────────────────── + # Instead of creating effective_friction copy, apply directly to cost + + # Start with base friction if friction is not None: if friction.shape != elevation.shape: - raise ValueError( - f"Friction shape {friction.shape} does not match elevation shape {elevation.shape}" - ) - effective_friction = friction.copy() - else: - effective_friction = np.ones(elevation.shape, dtype=np.float32) + raise ValueError(f"Friction shape mismatch") + np.multiply(cost, friction, out=cost) - # Apply trail burn-in: trails REPLACE land cover friction + # ─── Mode-specific terrain friction overrides (memory-efficient) ───────── + if friction_raw is not None and profile.terrain_friction_override: + if friction_raw.shape != elevation.shape: + raise ValueError(f"Friction_raw shape mismatch") + + # Process all overrides without creating large intermediate masks + for wc_class, override in profile.terrain_friction_override.items(): + if override is not None: + if override == np.inf: + # Use np.where for in-place-like behavior + np.putmask(cost, friction_raw == wc_class, np.inf) + else: + # Multiply cost where friction_raw matches + # Using a loop with putmask is more memory efficient + mask = friction_raw == wc_class + cost[mask] *= override + del mask + + # ─── Vehicle mode: allow flat grassland/cropland ───────────────────────── + if mode == "vehicle" and profile.off_trail_flat_threshold_deg > 0: + if friction_raw is not None: + # Compute slope in degrees for flat terrain check + slope_deg = np.degrees(np.arctan(grade)) + # Flat grassland or cropland - recompute cost for these cells + flat_field_mask = ( + (slope_deg <= profile.off_trail_flat_threshold_deg) & + ((friction_raw == 30) | (friction_raw == 40)) + ) + del slope_deg + # Recalculate cost for these cells with flat field friction + if np.any(flat_field_mask): + base_time = avg_cell_size * 3.6 / linear_degrade_speed( + grade[flat_field_mask], profile.base_speed_kmh, max_grade_val + ) + cost[flat_field_mask] = base_time * profile.off_trail_flat_friction + del base_time + del flat_field_mask + + # ─── Trail friction (mode-specific) ────────────────────────────────────── if trails is not None: if trails.shape != elevation.shape: - raise ValueError( - f"Trails shape {trails.shape} does not match elevation shape {elevation.shape}" - ) - # Replace friction where trails exist - for trail_value, trail_friction in TRAIL_FRICTION_MAP.items(): - trail_mask = trails == trail_value - effective_friction[trail_mask] = trail_friction + raise ValueError(f"Trails shape mismatch") - # Apply friction to cost - cost = cost * effective_friction + for trail_value, trail_friction in profile.trail_friction.items(): + if trail_friction is None: + # Impassable for this mode + np.putmask(cost, trails == trail_value, np.inf) + else: + # Trail friction REPLACES terrain friction + # Recalculate cost = base_time * trail_friction + trail_mask = trails == trail_value + if np.any(trail_mask): + # Get base travel time (without friction) + if profile.speed_function == "tobler": + trail_speed = tobler_off_path_speed(grade[trail_mask], profile.base_speed_kmh) + elif profile.speed_function == "herzog": + trail_speed = herzog_wheeled_speed(grade[trail_mask], profile.base_speed_kmh) + else: + trail_speed = linear_degrade_speed( + grade[trail_mask], profile.base_speed_kmh, max_grade_val + ) + cost[trail_mask] = (avg_cell_size * 3.6 / trail_speed) * trail_friction + del trail_speed + del trail_mask - # Apply barriers based on boundary_mode + # ─── Wilderness areas (mode-specific) ──────────────────────────────────── + if wilderness is not None and profile.wilderness_impassable: + if wilderness.shape != elevation.shape: + raise ValueError(f"Wilderness shape mismatch") + np.putmask(cost, wilderness == 255, np.inf) + + # ─── Barriers (private land) ───────────────────────────────────────────── if barriers is not None and boundary_mode != "emergency": if barriers.shape != elevation.shape: - raise ValueError( - f"Barriers shape {barriers.shape} does not match elevation shape {elevation.shape}" - ) - - barrier_mask = barriers == 255 + raise ValueError(f"Barriers shape mismatch") if boundary_mode == "strict": - # Mark closed/restricted areas as impassable - cost[barrier_mask] = np.inf + np.putmask(cost, barriers == 255, np.inf) elif boundary_mode == "pragmatic": - # Apply friction penalty to closed/restricted areas - cost[barrier_mask] = cost[barrier_mask] * PRAGMATIC_BARRIER_MULTIPLIER + barrier_mask = barriers == 255 + cost[barrier_mask] *= PRAGMATIC_BARRIER_MULTIPLIER + del barrier_mask return cost +# ═══════════════════════════════════════════════════════════════════════════════ +# LEGACY API (backward compatibility) +# ═══════════════════════════════════════════════════════════════════════════════ + +def tobler_speed(grade: float) -> float: + """Legacy single-value Tobler speed function.""" + return 0.6 * 6.0 * math.exp(-3.5 * abs(grade + 0.05)) + + +# ═══════════════════════════════════════════════════════════════════════════════ +# TESTING +# ═══════════════════════════════════════════════════════════════════════════════ + if __name__ == "__main__": - print("Testing Tobler speed function:") - for grade in [-0.3, -0.1, -0.05, 0.0, 0.05, 0.1, 0.3]: - speed = tobler_speed(grade) - print(f" Grade {grade:+.2f}: {speed:.2f} km/h") + print("=" * 70) + print("OFFROUTE Multi-Mode Cost Function Tests") + print("=" * 70) - print("\nTesting cost grid computation (no friction, no trails):") - elev = np.arange(100).reshape(10, 10).astype(np.float32) * 10 - cost = compute_cost_grid(elev, cell_size_m=30.0) - print(f" Elevation range: {elev.min():.0f} - {elev.max():.0f} m") - finite = cost[~np.isinf(cost)] - if len(finite) > 0: - print(f" Cost range: {finite.min():.1f} - {finite.max():.1f} s") - else: - print(f" All cells impassable (test data too steep)") + print("\n[1] Speed functions at various grades:") + print(f"{'Grade':<10} {'Foot':<12} {'MTB':<12} {'ATV':<12} {'Vehicle':<12}") + print("-" * 60) - print("\nTesting cost grid with friction and trails:") - elev = np.ones((10, 10), dtype=np.float32) * 1000 # flat terrain - friction = np.ones((10, 10), dtype=np.float32) * 2.0 # 2.0x friction (forest) - trails = np.zeros((10, 10), dtype=np.uint8) - trails[5, :] = 5 # road across middle row + for grade_val in [-0.3, -0.1, 0.0, 0.1, 0.2, 0.3]: + grade_arr = np.array([grade_val]) + foot = tobler_off_path_speed(grade_arr, 6.0)[0] + mtb = herzog_wheeled_speed(grade_arr, 12.0)[0] + atv = herzog_wheeled_speed(grade_arr, 25.0)[0] + veh = linear_degrade_speed(grade_arr, 40.0, np.tan(np.radians(20)))[0] + print(f"{grade_val:+.2f} {foot:>6.2f} km/h {mtb:>6.2f} km/h {atv:>6.2f} km/h {veh:>6.2f} km/h") - cost_no_trail = compute_cost_grid(elev, cell_size_m=30.0, friction=friction) - cost_with_trail = compute_cost_grid(elev, cell_size_m=30.0, friction=friction, trails=trails) + print("\n[2] Mode profiles:") + for name, profile in MODE_PROFILES.items(): + print(f"\n {name.upper()}: {profile.description}") + print(f" Max slope: {profile.max_slope_deg}°") + print(f" Trail access: {profile.trail_friction}") + print(f" Wilderness blocked: {profile.wilderness_impassable}") - base_cost = 30 * 3.6 / (0.6 * 6.0 * np.exp(-3.5 * 0.05)) - print(f" Base cost (flat, 30m cell): {base_cost:.1f} s") - print(f" Forest cell (2.0x friction): {cost_no_trail[0, 0]:.1f} s") - print(f" Road cell (0.1x friction, replaces forest): {cost_with_trail[5, 0]:.1f} s") - print(f" Road friction advantage: {cost_no_trail[0, 0] / cost_with_trail[5, 0]:.1f}x faster") - - print("\nTesting cost grid with barriers (three modes):") + print("\n[3] Cost grid test (flat terrain, forest):") elev = np.ones((10, 10), dtype=np.float32) * 1000 - barriers = np.zeros((10, 10), dtype=np.uint8) - barriers[3:7, 3:7] = 255 + friction = np.ones((10, 10), dtype=np.float32) * 2.0 # Forest friction + friction_raw = np.ones((10, 10), dtype=np.uint8) * 10 # Tree cover class - for mode in ["strict", "pragmatic", "emergency"]: - cost = compute_cost_grid(elev, cell_size_m=30.0, barriers=barriers, boundary_mode=mode) + trails = np.zeros((10, 10), dtype=np.uint8) + trails[5, :] = 5 # Road across middle + + for mode_name in ["foot", "mtb", "atv", "vehicle"]: + cost = compute_cost_grid( + elev, cell_size_m=30.0, + friction=friction, + friction_raw=friction_raw, + trails=trails, + mode=mode_name + ) + off_trail_cost = cost[0, 0] + road_cost = cost[5, 0] impassable = np.sum(np.isinf(cost)) - barrier_cost = cost[5, 5] if not np.isinf(cost[5, 5]) else "inf" - print(f" {mode:10s}: {impassable} impassable, barrier cell cost = {barrier_cost}") + print(f" {mode_name:8s}: off-trail={off_trail_cost:>8.1f}s, road={road_cost:>6.1f}s, impassable={impassable}") + + print("\n[4] Wilderness blocking test:") + wilderness = np.zeros((10, 10), dtype=np.uint8) + wilderness[3:7, 3:7] = 255 + + for mode_name in ["foot", "mtb", "atv", "vehicle"]: + cost = compute_cost_grid( + elev, cell_size_m=30.0, + wilderness=wilderness, + mode=mode_name + ) + wilderness_impassable = np.sum(np.isinf(cost[3:7, 3:7])) + print(f" {mode_name:8s}: wilderness cells impassable = {wilderness_impassable}/16") + + print("\nDone.") diff --git a/lib/offroute/router.py b/lib/offroute/router.py index 57d6ce5..d44bbb4 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -6,6 +6,8 @@ Connects the raster pathfinder (wilderness segment) to Valhalla (on-network segm Entry points are extracted from OSM highways and stored in /mnt/nav/navi.db. The pathfinder routes from a wilderness start to the nearest entry point, then Valhalla completes the route to the destination. + +Supports four travel modes: foot, mtb, atv, vehicle. """ import json import math @@ -23,7 +25,7 @@ from skimage.graph import MCP_Geometric from .dem import DEMReader from .cost import compute_cost_grid from .friction import FrictionReader, friction_to_multiplier -from .barriers import BarrierReader +from .barriers import BarrierReader, WildernessReader, DEFAULT_WILDERNESS_PATH from .trails import TrailReader # Paths @@ -45,6 +47,7 @@ MODE_TO_COSTING = { "foot": "pedestrian", "mtb": "bicycle", "atv": "auto", + "vehicle": "auto", } @@ -120,7 +123,6 @@ class EntryPointIndex: def query_radius(self, lat: float, lon: float, radius_km: float) -> List[Dict]: """Query entry points within radius of a point.""" - # Approximate bbox for the radius lat_delta = radius_km / 111.0 lon_delta = radius_km / (111.0 * math.cos(math.radians(lat))) @@ -129,7 +131,6 @@ class EntryPointIndex: lon - lon_delta, lon + lon_delta ) - # Filter by actual distance and add distance field result = [] for p in points: dist = haversine_distance(lat, lon, p['lat'], p['lon']) @@ -140,17 +141,12 @@ class EntryPointIndex: return sorted(result, key=lambda x: x['distance_m']) def build_index(self, osm_pbf_path: Path = OSM_PBF_PATH) -> Dict: - """ - Build the entry point index from OSM PBF. - - Extracts endpoints of highway features that connect to the network. - """ + """Build the entry point index from OSM PBF.""" if not osm_pbf_path.exists(): raise FileNotFoundError(f"OSM PBF not found: {osm_pbf_path}") print(f"Building trail entry point index from {osm_pbf_path}...") - # Highway types to extract (routable network entry points) highway_types = [ "primary", "secondary", "tertiary", "unclassified", "residential", "service", "track", "path", "footway", "bridleway" @@ -159,42 +155,29 @@ class EntryPointIndex: stats = {"total": 0, "by_class": {}} with tempfile.TemporaryDirectory() as tmpdir: - # Extract highways to GeoJSON geojson_path = Path(tmpdir) / "highways.geojson" - # Build osmium tags-filter expressions (one per highway type) print(f" Extracting highways with osmium...") - cmd = [ - "osmium", "tags-filter", - str(osm_pbf_path), - ] - # Add each highway type as a separate filter expression + cmd = ["osmium", "tags-filter", str(osm_pbf_path)] for ht in highway_types: cmd.append(f"w/highway={ht}") cmd.extend(["-o", str(Path(tmpdir) / "filtered.osm.pbf"), "--overwrite"]) - subprocess.run(cmd, check=True, capture_output=True) - # Convert to GeoJSON print(f" Converting to GeoJSON with ogr2ogr...") cmd = [ "ogr2ogr", "-f", "GeoJSON", str(geojson_path), str(Path(tmpdir) / "filtered.osm.pbf"), - "lines", - "-t_srs", "EPSG:4326" + "lines", "-t_srs", "EPSG:4326" ] subprocess.run(cmd, check=True, capture_output=True) - # Parse GeoJSON and extract endpoints print(f" Extracting entry points...") with open(geojson_path) as f: data = json.load(f) - # Collect unique points (endpoints) - # Key: (lat, lon) rounded to 5 decimal places (~1m precision) points = {} - for feature in data.get("features", []): props = feature.get("properties", {}) geom = feature.get("geometry", {}) @@ -209,62 +192,48 @@ class EntryPointIndex: highway_class = props.get("highway", "unknown") name = props.get("name", "") - # Extract endpoints for coord in [coords[0], coords[-1]]: lon, lat = coord[0], coord[1] key = (round(lat, 5), round(lon, 5)) if key not in points: points[key] = { - "lat": lat, - "lon": lon, - "highway_class": highway_class, - "name": name + "lat": lat, "lon": lon, + "highway_class": highway_class, "name": name } else: - # Keep the "best" highway class (roads > tracks > paths) existing = points[key] if self._highway_priority(highway_class) < self._highway_priority(existing["highway_class"]): points[key]["highway_class"] = highway_class if name and not existing["name"]: points[key]["name"] = name - # Create/update database print(f" Writing {len(points)} entry points to {self.db_path}...") self.db_path.parent.mkdir(parents=True, exist_ok=True) conn = self._get_conn() - # Create table conn.execute(""" CREATE TABLE IF NOT EXISTS trail_entry_points ( id INTEGER PRIMARY KEY AUTOINCREMENT, - lat REAL NOT NULL, - lon REAL NOT NULL, - highway_class TEXT NOT NULL, - name TEXT + lat REAL NOT NULL, lon REAL NOT NULL, + highway_class TEXT NOT NULL, name TEXT ) """) - - # Clear existing data conn.execute("DELETE FROM trail_entry_points") - # Insert new points for point in points.values(): - conn.execute(""" - INSERT INTO trail_entry_points (lat, lon, highway_class, name) - VALUES (?, ?, ?, ?) - """, (point["lat"], point["lon"], point["highway_class"], point["name"])) - + conn.execute( + "INSERT INTO trail_entry_points (lat, lon, highway_class, name) VALUES (?, ?, ?, ?)", + (point["lat"], point["lon"], point["highway_class"], point["name"]) + ) stats["total"] += 1 hc = point["highway_class"] stats["by_class"][hc] = stats["by_class"].get(hc, 0) + 1 - # Create spatial index conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lat ON trail_entry_points(lat)") conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lon ON trail_entry_points(lon)") conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_latlon ON trail_entry_points(lat, lon)") - conn.commit() print(f" Done. Total: {stats['total']} entry points") @@ -291,12 +260,15 @@ class EntryPointIndex: class OffrouteRouter: """ OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. + + Supports modes: foot, mtb, atv, vehicle """ def __init__(self): self.dem_reader = None self.friction_reader = None self.barrier_reader = None + self.wilderness_reader = None self.trail_reader = None self.entry_index = EntryPointIndex() @@ -308,6 +280,8 @@ class OffrouteRouter: self.friction_reader = FrictionReader() if self.barrier_reader is None: self.barrier_reader = BarrierReader() + if self.wilderness_reader is None and DEFAULT_WILDERNESS_PATH.exists(): + self.wilderness_reader = WildernessReader() if self.trail_reader is None: self.trail_reader = TrailReader() @@ -317,16 +291,25 @@ class OffrouteRouter: start_lon: float, end_lat: float, end_lon: float, - mode: Literal["foot", "mtb", "atv"] = "foot", + mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot", boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" ) -> Dict: """ Route from a wilderness start point to a destination. + Args: + start_lat, start_lon: Starting coordinates (wilderness) + end_lat, end_lon: Destination coordinates + mode: Travel mode (foot, mtb, atv, vehicle) + boundary_mode: How to handle private land (strict, pragmatic, emergency) + Returns a GeoJSON FeatureCollection with wilderness and network segments. """ t0 = time.time() + if mode not in MODE_TO_COSTING: + return {"status": "error", "message": f"Unknown mode: {mode}"} + # Ensure entry point index exists if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: return { @@ -334,28 +317,27 @@ class OffrouteRouter: "message": "Trail entry point index not built. Run build_entry_index() first." } - # Find entry points near start - entry_points = self.entry_index.query_radius( - start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM - ) + # Find entry points near start (limit to nearest 10 to control bbox size) + MAX_ENTRY_POINTS = 10 + entry_points = self.entry_index.query_radius(start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM) if not entry_points: - # Try expanded radius - entry_points = self.entry_index.query_radius( - start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM - ) + entry_points = self.entry_index.query_radius(start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM) if not entry_points: return { "status": "error", "message": f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start" } - # Build bbox for pathfinding grid - # Include start, end, and all entry points + # Limit to nearest entry points to prevent huge bounding boxes + entry_points = entry_points[:MAX_ENTRY_POINTS] + + # Build bbox with max size limit (prevent OOM on large areas) + MAX_BBOX_DEGREES = 0.5 # ~55km at mid-latitudes all_lats = [start_lat, end_lat] + [p["lat"] for p in entry_points] all_lons = [start_lon, end_lon] + [p["lon"] for p in entry_points] - padding = 0.05 # ~5km padding + padding = 0.05 bbox = { "south": min(all_lats) - padding, "north": max(all_lats) + padding, @@ -363,16 +345,28 @@ class OffrouteRouter: "east": max(all_lons) + padding, } + # Clamp bbox size to prevent memory exhaustion + lat_span = bbox["north"] - bbox["south"] + lon_span = bbox["east"] - bbox["west"] + if lat_span > MAX_BBOX_DEGREES or lon_span > MAX_BBOX_DEGREES: + center_lat = (bbox["south"] + bbox["north"]) / 2 + center_lon = (bbox["west"] + bbox["east"]) / 2 + half_span = MAX_BBOX_DEGREES / 2 + bbox = { + "south": center_lat - half_span, + "north": center_lat + half_span, + "west": center_lon - half_span, + "east": center_lon + half_span, + } + # Initialize readers self._init_readers() # Load elevation try: elevation, meta = self.dem_reader.get_elevation_grid( - south=bbox["south"], - north=bbox["north"], - west=bbox["west"], - east=bbox["east"], + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], ) except Exception as e: return {"status": "error", "message": f"Failed to load elevation: {e}"} @@ -382,62 +376,69 @@ class OffrouteRouter: if mem > MEMORY_LIMIT_GB: return {"status": "error", "message": f"Memory limit exceeded: {mem:.1f}GB > {MEMORY_LIMIT_GB}GB"} - # Load friction + # Load friction (both processed and raw for mode-specific overrides) friction_raw = self.friction_reader.get_friction_grid( - south=bbox["south"], - north=bbox["north"], - west=bbox["west"], - east=bbox["east"], + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], target_shape=elevation.shape ) friction_mult = friction_to_multiplier(friction_raw) # Load barriers barriers = self.barrier_reader.get_barrier_grid( - south=bbox["south"], - north=bbox["north"], - west=bbox["west"], - east=bbox["east"], + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], target_shape=elevation.shape ) + # Load wilderness (if available and mode requires it) + wilderness = None + if self.wilderness_reader is not None and mode in ("mtb", "atv", "vehicle"): + wilderness = self.wilderness_reader.get_wilderness_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape + ) + # Load trails trails = self.trail_reader.get_trails_grid( - south=bbox["south"], - north=bbox["north"], - west=bbox["west"], - east=bbox["east"], + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], target_shape=elevation.shape ) - # Compute cost grid + # Compute cost grid with mode-specific parameters cost = compute_cost_grid( elevation, cell_size_m=meta["cell_size_m"], friction=friction_mult, + friction_raw=friction_raw, trails=trails, barriers=barriers, + wilderness=wilderness, boundary_mode=boundary_mode, + mode=mode, ) + # Free intermediate arrays to reduce memory before MCP + # Note: Keep trails and barriers - needed for path statistics + del friction_mult, friction_raw, wilderness + import gc + gc.collect() + # Convert start to pixel coordinates start_row, start_col = self.dem_reader.latlon_to_pixel(start_lat, start_lon, meta) - # Validate start is in bounds rows, cols = elevation.shape if not (0 <= start_row < rows and 0 <= start_col < cols): return {"status": "error", "message": "Start point outside grid bounds"} - # Mark entry points on the grid + # Mark entry points on grid entry_pixels = [] for ep in entry_points: row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) if 0 <= row < rows and 0 <= col < cols: - entry_pixels.append({ - "row": row, - "col": col, - "entry_point": ep - }) + entry_pixels.append({"row": row, "col": col, "entry_point": ep}) if not entry_pixels: return {"status": "error", "message": "No entry points map to grid bounds"} @@ -465,18 +466,21 @@ class OffrouteRouter: # Traceback wilderness path path_indices = mcp.traceback((best_entry["row"], best_entry["col"])) - # Convert to coordinates and collect stats + # Convert to coordinates wilderness_coords = [] elevations = [] trail_values = [] + barrier_crossings = 0 for row, col in path_indices: lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) wilderness_coords.append([lon, lat]) elevations.append(elevation[row, col]) trail_values.append(trails[row, col]) + if barriers[row, col] == 255: + barrier_crossings += 1 - # Calculate wilderness segment stats + # Calculate stats wilderness_distance_m = 0 for i in range(1, len(wilderness_coords)): lon1, lat1 = wilderness_coords[i-1] @@ -493,13 +497,16 @@ class OffrouteRouter: total_cells = len(trail_arr) on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 - # Entry point reached + # Free trails and barriers now that path stats are computed + del trails, barriers + + # Entry point entry_lat = best_entry["entry_point"]["lat"] entry_lon = best_entry["entry_point"]["lon"] entry_class = best_entry["entry_point"]["highway_class"] entry_name = best_entry["entry_point"].get("name", "") - # Call Valhalla for on-network segment + # Call Valhalla valhalla_costing = MODE_TO_COSTING.get(mode, "pedestrian") valhalla_request = { @@ -515,11 +522,7 @@ class OffrouteRouter: valhalla_error = None try: - resp = requests.post( - f"{VALHALLA_URL}/route", - json=valhalla_request, - timeout=30 - ) + resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) if resp.status_code == 200: valhalla_data = resp.json() @@ -529,11 +532,8 @@ class OffrouteRouter: if legs: leg = legs[0] shape = leg.get("shape", "") - - # Decode polyline6 network_coords = self._decode_polyline(shape) - # Extract maneuvers maneuvers = [] for m in leg.get("maneuvers", []): maneuvers.append({ @@ -560,7 +560,6 @@ class OffrouteRouter: # Build response features = [] - # Feature 1: Wilderness segment wilderness_feature = { "type": "Feature", "properties": { @@ -572,15 +571,13 @@ class OffrouteRouter: "boundary_mode": boundary_mode, "on_trail_pct": on_trail_pct, "cell_count": total_cells, + "barrier_crossings": barrier_crossings, + "mode": mode, }, - "geometry": { - "type": "LineString", - "coordinates": wilderness_coords, - } + "geometry": {"type": "LineString", "coordinates": wilderness_coords} } features.append(wilderness_feature) - # Feature 2: Network segment (if available) if network_segment: network_feature = { "type": "Feature", @@ -590,40 +587,23 @@ class OffrouteRouter: "duration_minutes": network_segment["duration_minutes"], "maneuvers": network_segment["maneuvers"], }, - "geometry": { - "type": "LineString", - "coordinates": network_segment["coordinates"], - } + "geometry": {"type": "LineString", "coordinates": network_segment["coordinates"]} } features.append(network_feature) - # Build combined route coordinates combined_coords = wilderness_coords.copy() if network_segment: - # Skip first point of network segment (it's the same as last wilderness point) combined_coords.extend(network_segment["coordinates"][1:]) - # Feature 3: Combined route combined_feature = { "type": "Feature", - "properties": { - "segment_type": "combined", - "mode": mode, - "boundary_mode": boundary_mode, - }, - "geometry": { - "type": "LineString", - "coordinates": combined_coords, - } + "properties": {"segment_type": "combined", "mode": mode, "boundary_mode": boundary_mode}, + "geometry": {"type": "LineString", "coordinates": combined_coords} } features.append(combined_feature) - geojson = { - "type": "FeatureCollection", - "features": features, - } + geojson = {"type": "FeatureCollection", "features": features} - # Build summary total_distance_km = wilderness_distance_m / 1000 total_effort_minutes = best_cost / 60 @@ -639,22 +619,17 @@ class OffrouteRouter: "network_distance_km": float(network_segment["distance_km"]) if network_segment else 0, "network_duration_minutes": float(network_segment["duration_minutes"]) if network_segment else 0, "on_trail_pct": on_trail_pct, + "barrier_crossings": barrier_crossings, "boundary_mode": boundary_mode, "mode": mode, "entry_point": { - "lat": entry_lat, - "lon": entry_lon, - "highway_class": entry_class, - "name": entry_name, + "lat": entry_lat, "lon": entry_lon, + "highway_class": entry_class, "name": entry_name, }, "computation_time_s": time.time() - t0, } - result = { - "status": "ok", - "route": geojson, - "summary": summary, - } + result = {"status": "ok", "route": geojson, "summary": summary} if valhalla_error: result["warning"] = f"Network segment incomplete: {valhalla_error}" @@ -669,7 +644,6 @@ class OffrouteRouter: lon = 0 while index < len(encoded): - # Latitude shift = 0 result = 0 while True: @@ -682,7 +656,6 @@ class OffrouteRouter: dlat = ~(result >> 1) if result & 1 else result >> 1 lat += dlat - # Longitude shift = 0 result = 0 while True: @@ -707,6 +680,8 @@ class OffrouteRouter: self.friction_reader.close() if self.barrier_reader: self.barrier_reader.close() + if self.wilderness_reader: + self.wilderness_reader.close() if self.trail_reader: self.trail_reader.close() self.entry_index.close() @@ -729,24 +704,33 @@ if __name__ == "__main__": print(f"\nDone. Total entry points: {stats['total']}") elif len(sys.argv) > 1 and sys.argv[1] == "test": - print("Testing router...") + print("Testing router (all modes)...") router = OffrouteRouter() - # Test route: wilderness to Twin Falls - result = router.route( - start_lat=42.35, - start_lon=-114.30, - end_lat=42.5629, - end_lon=-114.4609, - mode="foot", - boundary_mode="pragmatic" - ) + for mode in ["foot", "mtb", "atv", "vehicle"]: + print(f"\n{'='*60}") + print(f"Mode: {mode}") + print("="*60) + + result = router.route( + start_lat=42.35, start_lon=-114.30, + end_lat=42.5629, end_lon=-114.4609, + mode=mode, boundary_mode="pragmatic" + ) + + if result["status"] == "ok": + s = result["summary"] + print(f" Wilderness: {s['wilderness_distance_km']:.2f} km, {s['wilderness_effort_minutes']:.1f} min") + print(f" Network: {s['network_distance_km']:.2f} km, {s['network_duration_minutes']:.1f} min") + print(f" On-trail: {s['on_trail_pct']:.1f}%") + print(f" Entry: {s['entry_point']['highway_class']}") + else: + print(f" ERROR: {result['message']}") - print(json.dumps(result, indent=2, default=str)) router.close() else: print("Usage:") print(" python router.py build # Build entry point index") - print(" python router.py test # Test route") + print(" python router.py test # Test all modes") From 2252905986ed21264a0c873f3037a25fc63543b9 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 14:26:18 +0000 Subject: [PATCH 07/16] =?UTF-8?q?feat(offroute):=20MVUM=20legal=20access?= =?UTF-8?q?=20=E2=80=94=20pathfinder=20integration=20+=20places=20panel=20?= =?UTF-8?q?API=20+=20boundary=5Fmode=20control?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MVUM Data Import: - Downloaded USFS MVUM Roads (150,636 features) and Trails (28,741 features) - Imported to navi.db as mvum_roads and mvum_trails tables - Idaho coverage: ~8,994 roads and ~4,504 trails across 7 national forests - Preserved all vehicle-class fields (ATV, MOTORCYCLE, HIGHCLEARANCEVEHICLE, etc.) - Preserved seasonal date ranges (*_DATESOPEN fields) New mvum.py module: - MVUMReader class for querying MVUM data by bbox and nearest point - parse_date_range() for seasonal date string parsing (MM/DD-MM/DD format) - check_access() for determining open/closed status with date checking - symbol_to_access() fallback when per-vehicle fields are null - get_mvum_access_grid() for rasterizing MVUM to pathfinder grid Cost function integration: - Added mvum parameter to compute_cost_grid() - MVUM closures respond to boundary_mode: * strict = impassable (np.inf) * pragmatic = 5x friction penalty * emergency = ignored entirely - Foot mode skips MVUM (motor-vehicle specific) Router integration: - Loads MVUM access grid for motorized modes (mtb, atv, vehicle) - Tracks mvum_closed_crossings in path summary Places Panel API: - GET /api/mvum?lat=XX&lon=XX&radius=50 - Returns MVUM feature with access status for all vehicle classes - Includes seasonal date ranges, maintenance level, forest/district info - GeoJSON geometry for map display Validation: - MVUM places endpoint tested with Sawtooth NF road - All four modes validated with strict/pragmatic/emergency boundary modes - Foot mode correctly ignores MVUM restrictions Co-Authored-By: Claude Opus 4.5 --- lib/api.py | 134 +++++++++ lib/offroute/cost.py | 25 ++ lib/offroute/mvum.py | 623 +++++++++++++++++++++++++++++++++++++++++ lib/offroute/router.py | 29 +- 4 files changed, 809 insertions(+), 2 deletions(-) create mode 100644 lib/offroute/mvum.py diff --git a/lib/api.py b/lib/api.py index a127866..699d09d 100644 --- a/lib/api.py +++ b/lib/api.py @@ -2799,3 +2799,137 @@ def api_offroute(): except Exception as e: logger.exception("Offroute error") return jsonify({"status": "error", "message": str(e)}), 500 + + +# ── MVUM Places Panel API ── + +@app.route("/api/mvum", methods=["GET"]) +def api_mvum(): + """ + Query MVUM (Motor Vehicle Use Map) features near a point. + + Used by the Navi frontend places panel when a user taps near a road/trail. + + Query params: + lat: Latitude + lon: Longitude + radius: Search radius in meters (default: 50) + + Response: + { + "status": "ok", + "feature": { + "id": "FR 123", + "name": "Some Forest Road", + "forest": "Sawtooth National Forest", + "district": "Ketchum Ranger District", + "surface": "NAT", + "maintenance_level": 2, + "seasonal": "Seasonal", + "symbol": 2, + "access": { + "passenger_vehicle": { "status": "Open", "dates": "06/15-10/15" }, + "high_clearance": { "status": "Open", "dates": "06/15-10/15" }, + "atv": { "status": "Open", "dates": "06/15-10/15" }, + ... + }, + "geometry": { GeoJSON LineString } + } + } + + If no MVUM feature within radius: + { "status": "ok", "feature": null } + """ + try: + lat = request.args.get("lat", type=float) + lon = request.args.get("lon", type=float) + radius = request.args.get("radius", 50, type=float) + + if lat is None or lon is None: + return jsonify({"status": "error", "message": "lat and lon required"}), 400 + + from .offroute.mvum import MVUMReader + + reader = MVUMReader() + try: + # Try roads first, then trails + feature = reader.query_nearest(lat, lon, radius, "mvum_roads") + if feature is None: + feature = reader.query_nearest(lat, lon, radius, "mvum_trails") + + if feature is None: + return jsonify({"status": "ok", "feature": None}) + + # Format access info + access = { + "passenger_vehicle": { + "status": feature.get("passengervehicle"), + "dates": feature.get("passengervehicle_datesopen") + }, + "high_clearance": { + "status": feature.get("highclearancevehicle"), + "dates": feature.get("highclearancevehicle_datesopen") + }, + "atv": { + "status": feature.get("atv"), + "dates": feature.get("atv_datesopen") + }, + "motorcycle": { + "status": feature.get("motorcycle"), + "dates": feature.get("motorcycle_datesopen") + }, + "4wd_gt50": { + "status": feature.get("fourwd_gt50inches"), + "dates": feature.get("fourwd_gt50_datesopen") + }, + "2wd_gt50": { + "status": feature.get("twowd_gt50inches"), + "dates": feature.get("twowd_gt50_datesopen") + }, + "e_bike_class1": { + "status": feature.get("e_bike_class1"), + "dates": feature.get("e_bike_class1_dur") + }, + "e_bike_class2": { + "status": feature.get("e_bike_class2"), + "dates": feature.get("e_bike_class2_dur") + }, + "e_bike_class3": { + "status": feature.get("e_bike_class3"), + "dates": feature.get("e_bike_class3_dur") + }, + } + + # Parse maintenance level + maint_level = feature.get("operationalmaintlevel", "") + maint_num = None + if maint_level: + # Extract first digit: "2 - HIGH CLEARANCE VEHICLES" -> 2 + import re + match = re.match(r"(\d+)", maint_level) + if match: + maint_num = int(match.group(1)) + + result = { + "id": feature.get("id"), + "name": feature.get("name"), + "forest": feature.get("forestname"), + "district": feature.get("districtname"), + "surface": feature.get("surfacetype"), + "maintenance_level": maint_num, + "seasonal": feature.get("seasonal"), + "symbol": feature.get("symbol"), + "trail_class": feature.get("trailclass"), + "trail_system": feature.get("trailsystem"), + "access": access, + "geometry": feature.get("geojson") + } + + return jsonify({"status": "ok", "feature": result}) + + finally: + reader.close() + + except Exception as e: + logger.exception("MVUM query error") + return jsonify({"status": "error", "message": str(e)}), 500 diff --git a/lib/offroute/cost.py b/lib/offroute/cost.py index c3b6a5a..16b8514 100644 --- a/lib/offroute/cost.py +++ b/lib/offroute/cost.py @@ -213,6 +213,7 @@ def compute_cost_grid( trails: Optional[np.ndarray] = None, barriers: Optional[np.ndarray] = None, wilderness: Optional[np.ndarray] = None, + mvum: Optional[np.ndarray] = None, boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic", mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot" ) -> np.ndarray: @@ -236,6 +237,10 @@ def compute_cost_grid( 255 = closed/restricted area (PAD-US Pub_Access = XA). wilderness: Optional[np.ndarray] of wilderness values (uint8). 255 = designated wilderness area. + mvum: Optional[np.ndarray] of MVUM access values (uint8). + 0 = no MVUM data, 1 = open, 255 = closed to this mode. + MVUM closures respond to boundary_mode (strict/pragmatic/emergency). + Foot mode should pass None (MVUM is motor-vehicle specific). boundary_mode: How to handle barriers ("strict", "pragmatic", "emergency") mode: Travel mode ("foot", "mtb", "atv", "vehicle") @@ -392,6 +397,26 @@ def compute_cost_grid( cost[barrier_mask] *= PRAGMATIC_BARRIER_MULTIPLIER del barrier_mask + # ─── MVUM closures (motor vehicle restrictions) ────────────────────────── + # MVUM only applies to motorized modes, not foot. Foot mode should pass mvum=None. + # MVUM closures respond to the same boundary_mode as PAD-US barriers: + # "strict" = MVUM-closed road/trail is impassable + # "pragmatic" = MVUM-closed road/trail gets 5× friction penalty + # "emergency" = MVUM closures ignored entirely + if mvum is not None and mode != "foot" and boundary_mode != "emergency": + if mvum.shape != elevation.shape: + raise ValueError(f"MVUM shape mismatch") + + # Value 255 = road/trail exists but is closed to this mode + mvum_closed_mask = mvum == 255 + + if boundary_mode == "strict": + np.putmask(cost, mvum_closed_mask, np.inf) + elif boundary_mode == "pragmatic": + cost[mvum_closed_mask] *= PRAGMATIC_BARRIER_MULTIPLIER + + del mvum_closed_mask + return cost diff --git a/lib/offroute/mvum.py b/lib/offroute/mvum.py new file mode 100644 index 0000000..31e503d --- /dev/null +++ b/lib/offroute/mvum.py @@ -0,0 +1,623 @@ +""" +MVUM (Motor Vehicle Use Map) legal access layer for OFFROUTE. + +Queries USFS MVUM data from navi.db and provides rasterized access grids +indicating which roads/trails are open or closed to specific vehicle modes. + +MVUM is motor-vehicle specific — foot mode should skip this layer entirely. +""" +import re +import sqlite3 +import warnings +from datetime import datetime +from pathlib import Path +from typing import Dict, List, Optional, Tuple, Literal + +import numpy as np + +# Path to navi.db +NAVI_DB_PATH = Path("/mnt/nav/navi.db") + + +def parse_date_range(date_str: str) -> List[Tuple[int, int, int, int]]: + """ + Parse MVUM date range strings like "05/01-11/30" or "06/15-10/15,12/01-03/31". + + Returns list of (start_month, start_day, end_month, end_day) tuples. + Returns empty list if unparseable. + """ + if not date_str or date_str.strip() == "": + return [] + + ranges = [] + # Split by comma for multi-period strings + for part in date_str.split(","): + part = part.strip() + # Match MM/DD-MM/DD pattern + match = re.match(r"(\d{1,2})/(\d{1,2})-(\d{1,2})/(\d{1,2})", part) + if match: + try: + sm, sd, em, ed = int(match.group(1)), int(match.group(2)), int(match.group(3)), int(match.group(4)) + if 1 <= sm <= 12 and 1 <= sd <= 31 and 1 <= em <= 12 and 1 <= ed <= 31: + ranges.append((sm, sd, em, ed)) + except ValueError: + pass + + return ranges + + +def is_date_in_range(month: int, day: int, ranges: List[Tuple[int, int, int, int]]) -> bool: + """ + Check if a given month/day falls within any of the date ranges. + Handles ranges that wrap around year end (e.g., 12/01-03/31). + """ + if not ranges: + return True # No ranges = assume open + + date_num = month * 100 + day # Simple numeric comparison + + for sm, sd, em, ed in ranges: + start_num = sm * 100 + sd + end_num = em * 100 + ed + + if start_num <= end_num: + # Normal range (e.g., 05/01-11/30) + if start_num <= date_num <= end_num: + return True + else: + # Wrapping range (e.g., 12/01-03/31) + if date_num >= start_num or date_num <= end_num: + return True + + return False + + +def check_access( + status_field: Optional[str], + dates_field: Optional[str], + seasonal: Optional[str], + check_date: Optional[Tuple[int, int]] = None +) -> Optional[bool]: + """ + Determine if a road/trail is open to a vehicle type. + + Args: + status_field: Value of vehicle-class field (e.g., "open", null) + dates_field: Value of *_DATESOPEN field (e.g., "05/01-11/30") + seasonal: Value of SEASONAL field ("yearlong", "seasonal") + check_date: Optional (month, day) tuple to check against date ranges + + Returns: + True = open + False = closed + None = no data (field not populated, defer to SYMBOL) + """ + if status_field is None or status_field.strip() == "": + return None # No data + + status = status_field.strip().lower() + + if status != "open": + return False # Explicitly closed or restricted + + # Status is "open" - check seasonal restrictions + if check_date is not None: + month, day = check_date + + # Parse date ranges + if dates_field: + ranges = parse_date_range(dates_field) + if ranges: + return is_date_in_range(month, day, ranges) + + # No date field but seasonal = "yearlong" means always open + if seasonal and seasonal.strip().lower() == "yearlong": + return True + + # Seasonal with no dates - assume open (data quality issue) + if seasonal and seasonal.strip().lower() == "seasonal": + warnings.warn(f"Seasonal road/trail with no DATESOPEN, assuming open") + return True + + return True # Open with no date check + + +def get_mode_field(mode: str) -> Tuple[str, str]: + """ + Get the MVUM field names for a given travel mode. + + Returns (status_field, dates_field) tuple. + """ + mode_mapping = { + "atv": ("atv", "atv_datesopen"), + "motorcycle": ("motorcycle", "motorcycle_datesopen"), + "mtb": ("e_bike_class1", "e_bike_class1_dur"), # Closest analog for e-bikes + "vehicle": ("highclearancevehicle", "highclearancevehicle_datesopen"), + "passenger": ("passengervehicle", "passengervehicle_datesopen"), + } + + return mode_mapping.get(mode, ("highclearancevehicle", "highclearancevehicle_datesopen")) + + +def symbol_to_access(symbol: str, mode: str, maint_level: Optional[str] = None) -> Optional[bool]: + """ + Fallback: interpret SYMBOL field when per-vehicle-class fields are null. + + MVUM SYMBOL meanings (roads): + 1 = Open to all vehicles + 2 = Open to highway legal vehicles only + 3 = Road closed to motorized + 4 = Road open seasonally + 11 = Administrative use only + 12 = Decommissioned + + For trails, similar logic applies based on TRAILCLASS. + """ + if symbol is None: + return None + + sym = str(symbol).strip() + + # Symbol 1: Open to all + if sym == "1": + return True + + # Symbol 2: Highway legal only + if sym == "2": + # ATVs/motorcycles typically not highway legal + if mode in ("atv", "motorcycle"): + return False + return True + + # Symbol 3: Closed to motorized + if sym == "3": + return False + + # Symbol 4: Seasonally open (assume open if no date check) + if sym == "4": + return True + + # Symbol 11/12: Administrative/decommissioned = closed + if sym in ("11", "12"): + return False + + # Unknown symbol - defer + return None + + +class MVUMReader: + """ + Reader for MVUM data from navi.db. + + Queries roads and trails by bounding box and returns access grids. + """ + + def __init__(self, db_path: Path = NAVI_DB_PATH): + self.db_path = db_path + self._conn = None + + def _get_conn(self) -> sqlite3.Connection: + if self._conn is None: + if not self.db_path.exists(): + raise FileNotFoundError(f"navi.db not found at {self.db_path}") + self._conn = sqlite3.connect(str(self.db_path)) + self._conn.row_factory = sqlite3.Row + # Load Spatialite extension if available + try: + self._conn.enable_load_extension(True) + self._conn.load_extension("mod_spatialite") + except Exception: + pass # Spatialite not available, will use manual bbox queries + return self._conn + + def table_exists(self, table_name: str) -> bool: + """Check if an MVUM table exists.""" + conn = self._get_conn() + cur = conn.execute( + "SELECT name FROM sqlite_master WHERE type='table' AND name=?", + (table_name,) + ) + return cur.fetchone() is not None + + def query_roads_bbox( + self, + south: float, north: float, west: float, east: float, + mode: str = "atv", + check_date: Optional[Tuple[int, int]] = None + ) -> List[Dict]: + """ + Query MVUM roads within a bounding box. + + Returns list of dicts with access info for the given mode. + """ + if not self.table_exists("mvum_roads"): + return [] + + conn = self._get_conn() + + # Query using bbox on geometry + # Since we don't have spatialite, we'll query all and filter in Python + # For production, consider pre-computing bbox columns + cur = conn.execute(""" + SELECT ogc_fid, id, name, symbol, operationalmaintlevel, seasonal, + atv, atv_datesopen, motorcycle, motorcycle_datesopen, + highclearancevehicle, highclearancevehicle_datesopen, + passengervehicle, passengervehicle_datesopen, + e_bike_class1, e_bike_class1_dur, + shape + FROM mvum_roads + """) + + status_field, dates_field = get_mode_field(mode) + results = [] + + for row in cur: + # Parse geometry to check bbox intersection + # The shape is stored as WKB blob + shape = row["shape"] + if shape is None: + continue + + # Quick bbox check using geometry extent + # Since we don't have Spatialite functions, we'll include all + # and let the rasterization handle it + + access = check_access( + row[status_field] if status_field in row.keys() else None, + row[dates_field] if dates_field in row.keys() else None, + row["seasonal"], + check_date + ) + + # Fallback to SYMBOL if no per-vehicle data + if access is None: + access = symbol_to_access(row["symbol"], mode, row["operationalmaintlevel"]) + + if access is not None: + results.append({ + "id": row["id"], + "name": row["name"], + "access": access, + "symbol": row["symbol"], + "maint_level": row["operationalmaintlevel"], + "shape": shape, + }) + + return results + + def query_trails_bbox( + self, + south: float, north: float, west: float, east: float, + mode: str = "atv", + check_date: Optional[Tuple[int, int]] = None + ) -> List[Dict]: + """ + Query MVUM trails within a bounding box. + """ + if not self.table_exists("mvum_trails"): + return [] + + conn = self._get_conn() + + cur = conn.execute(""" + SELECT ogc_fid, id, name, symbol, seasonal, trailclass, + atv, atv_datesopen, motorcycle, motorcycle_datesopen, + highclearancevehicle, highclearancevehicle_datesopen, + passengervehicle, passengervehicle_datesopen, + e_bike_class1, e_bike_class1_dur, + shape + FROM mvum_trails + """) + + status_field, dates_field = get_mode_field(mode) + results = [] + + for row in cur: + shape = row["shape"] + if shape is None: + continue + + access = check_access( + row[status_field] if status_field in row.keys() else None, + row[dates_field] if dates_field in row.keys() else None, + row["seasonal"], + check_date + ) + + if access is None: + access = symbol_to_access(row["symbol"], mode) + + if access is not None: + results.append({ + "id": row["id"], + "name": row["name"], + "access": access, + "symbol": row["symbol"], + "trail_class": row["trailclass"], + "shape": shape, + }) + + return results + + def query_nearest( + self, + lat: float, lon: float, + radius_m: float = 50, + table: str = "mvum_roads" + ) -> Optional[Dict]: + """ + Query the nearest MVUM feature to a point. + + Used for the places panel API. + """ + if not self.table_exists(table): + return None + + conn = self._get_conn() + + # Convert radius to degrees (approximate) + radius_deg = radius_m / 111000 + + # Query features in bbox around point + if table == "mvum_roads": + cur = conn.execute(""" + SELECT ogc_fid, id, name, forestname, districtname, symbol, + operationalmaintlevel, surfacetype, seasonal, jurisdiction, + passengervehicle, passengervehicle_datesopen, + highclearancevehicle, highclearancevehicle_datesopen, + atv, atv_datesopen, motorcycle, motorcycle_datesopen, + fourwd_gt50inches, fourwd_gt50_datesopen, + twowd_gt50inches, twowd_gt50_datesopen, + e_bike_class1, e_bike_class1_dur, + e_bike_class2, e_bike_class2_dur, + e_bike_class3, e_bike_class3_dur, + shape + FROM mvum_roads + LIMIT 1000 + """) + else: + cur = conn.execute(""" + SELECT ogc_fid, id, name, forestname, districtname, symbol, + seasonal, jurisdiction, trailclass, trailsystem, + passengervehicle, passengervehicle_datesopen, + highclearancevehicle, highclearancevehicle_datesopen, + atv, atv_datesopen, motorcycle, motorcycle_datesopen, + fourwd_gt50inches, fourwd_gt50_datesopen, + twowd_gt50inches, twowd_gt50_datesopen, + e_bike_class1, e_bike_class1_dur, + e_bike_class2, e_bike_class2_dur, + e_bike_class3, e_bike_class3_dur, + shape + FROM mvum_trails + LIMIT 1000 + """) + + # Find nearest feature + # This is a simplified approach - for production, use spatial index + try: + from shapely import wkb + from shapely.geometry import Point + + query_point = Point(lon, lat) + nearest = None + min_dist = float('inf') + + for row in cur: + try: + geom = wkb.loads(row["shape"]) + dist = query_point.distance(geom) + if dist < min_dist and dist < radius_deg: + min_dist = dist + nearest = dict(row) + nearest["geometry"] = geom + except Exception: + continue + + if nearest: + # Convert geometry to GeoJSON + nearest["geojson"] = nearest["geometry"].__geo_interface__ + del nearest["geometry"] + del nearest["shape"] + return nearest + + except ImportError: + warnings.warn("shapely not available for nearest query") + + return None + + def close(self): + if self._conn: + self._conn.close() + self._conn = None + + +def get_mvum_access_grid( + south: float, north: float, west: float, east: float, + target_shape: Tuple[int, int], + mode: Literal["foot", "mtb", "atv", "vehicle"] = "atv", + check_date: Optional[str] = None, + db_path: Path = NAVI_DB_PATH +) -> np.ndarray: + """ + Get MVUM access grid for pathfinding. + + Args: + south, north, west, east: Bounding box (WGS84) + target_shape: (rows, cols) to match elevation grid + mode: Travel mode (foot skips MVUM entirely) + check_date: Optional "MM/DD" string for seasonal checking + db_path: Path to navi.db + + Returns: + np.ndarray of uint8: + 0 = no MVUM data (defer to existing trail/friction logic) + 1 = road/trail is OPEN to this vehicle mode + 255 = road/trail EXISTS but is CLOSED to this mode + """ + # Foot mode bypasses MVUM entirely + if mode == "foot": + return np.zeros(target_shape, dtype=np.uint8) + + # Parse check_date if provided + parsed_date = None + if check_date: + match = re.match(r"(\d{1,2})/(\d{1,2})", check_date) + if match: + parsed_date = (int(match.group(1)), int(match.group(2))) + + # Initialize output grid + grid = np.zeros(target_shape, dtype=np.uint8) + rows, cols = target_shape + + # Pixel size + pixel_lat = (north - south) / rows + pixel_lon = (east - west) / cols + + reader = MVUMReader(db_path) + + try: + # Query roads and trails + roads = reader.query_roads_bbox(south, north, west, east, mode, parsed_date) + trails = reader.query_trails_bbox(south, north, west, east, mode, parsed_date) + + # Rasterize features + try: + from shapely import wkb + + for features in [roads, trails]: + for feat in features: + try: + geom = wkb.loads(feat["shape"]) + + # Get geometry bounds + minx, miny, maxx, maxy = geom.bounds + + # Check if intersects our bbox + if maxx < west or minx > east or maxy < south or miny > north: + continue + + # Rasterize line + value = 1 if feat["access"] else 255 + + # Simple line rasterization + if geom.geom_type in ("LineString", "MultiLineString"): + if geom.geom_type == "MultiLineString": + coords_list = [list(line.coords) for line in geom.geoms] + else: + coords_list = [list(geom.coords)] + + for coords in coords_list: + for i in range(len(coords) - 1): + x1, y1 = coords[i] + x2, y2 = coords[i + 1] + + # Convert to pixel coordinates + col1 = int((x1 - west) / pixel_lon) + row1 = int((north - y1) / pixel_lat) + col2 = int((x2 - west) / pixel_lon) + row2 = int((north - y2) / pixel_lat) + + # Bresenham's line algorithm + _draw_line(grid, row1, col1, row2, col2, value) + + except Exception as e: + continue + + except ImportError: + warnings.warn("shapely not available, MVUM rasterization skipped") + + finally: + reader.close() + + return grid + + +def _draw_line(grid: np.ndarray, r1: int, c1: int, r2: int, c2: int, value: int): + """Draw a line on the grid using Bresenham's algorithm.""" + rows, cols = grid.shape + + dr = abs(r2 - r1) + dc = abs(c2 - c1) + sr = 1 if r1 < r2 else -1 + sc = 1 if c1 < c2 else -1 + err = dr - dc + + r, c = r1, c1 + + while True: + if 0 <= r < rows and 0 <= c < cols: + # Only overwrite if current value is 0 (no data) or we're marking closed + if grid[r, c] == 0 or value == 255: + grid[r, c] = value + + if r == r2 and c == c2: + break + + e2 = 2 * err + if e2 > -dc: + err -= dc + r += sr + if e2 < dr: + err += dr + c += sc + + +if __name__ == "__main__": + import sys + + print("=" * 60) + print("MVUM Reader Test") + print("=" * 60) + + reader = MVUMReader() + + if not reader.table_exists("mvum_roads"): + print("ERROR: mvum_roads table not found in navi.db") + sys.exit(1) + + # Test bbox query (Sawtooth NF area) + print("\n[1] Testing bbox query (Sawtooth NF area)...") + roads = reader.query_roads_bbox( + south=43.5, north=44.0, west=-115.0, east=-114.0, + mode="atv" + ) + print(f" Found {len(roads)} roads") + + open_count = sum(1 for r in roads if r["access"]) + closed_count = sum(1 for r in roads if not r["access"]) + print(f" Open to ATV: {open_count}") + print(f" Closed to ATV: {closed_count}") + + # Test with seasonal date + print("\n[2] Testing with date check (July 15)...") + roads_summer = reader.query_roads_bbox( + south=43.5, north=44.0, west=-115.0, east=-114.0, + mode="atv", + check_date=(7, 15) + ) + open_summer = sum(1 for r in roads_summer if r["access"]) + print(f" Open to ATV on 07/15: {open_summer}") + + print("\n[3] Testing with date check (January 15)...") + roads_winter = reader.query_roads_bbox( + south=43.5, north=44.0, west=-115.0, east=-114.0, + mode="atv", + check_date=(1, 15) + ) + open_winter = sum(1 for r in roads_winter if r["access"]) + print(f" Open to ATV on 01/15: {open_winter}") + + # Test grid generation + print("\n[4] Testing grid generation...") + grid = get_mvum_access_grid( + south=43.5, north=44.0, west=-115.0, east=-114.0, + target_shape=(500, 1000), + mode="atv" + ) + print(f" Grid shape: {grid.shape}") + print(f" No data (0): {np.sum(grid == 0)}") + print(f" Open (1): {np.sum(grid == 1)}") + print(f" Closed (255): {np.sum(grid == 255)}") + + reader.close() + print("\nDone.") diff --git a/lib/offroute/router.py b/lib/offroute/router.py index d44bbb4..c37bddf 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -27,6 +27,7 @@ from .cost import compute_cost_grid from .friction import FrictionReader, friction_to_multiplier from .barriers import BarrierReader, WildernessReader, DEFAULT_WILDERNESS_PATH from .trails import TrailReader +from .mvum import get_mvum_access_grid # Paths NAVI_DB_PATH = Path("/mnt/nav/navi.db") @@ -407,6 +408,22 @@ class OffrouteRouter: target_shape=elevation.shape ) + # Load MVUM access data (only for motorized modes) + # MVUM is motor-vehicle specific — foot mode skips entirely + mvum = None + if mode in ("mtb", "atv", "vehicle"): + try: + mvum = get_mvum_access_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape, + mode=mode, + check_date=None, # TODO: accept date parameter + ) + except Exception as e: + # MVUM data may not be available - continue without it + pass + # Compute cost grid with mode-specific parameters cost = compute_cost_grid( elevation, @@ -416,12 +433,13 @@ class OffrouteRouter: trails=trails, barriers=barriers, wilderness=wilderness, + mvum=mvum, boundary_mode=boundary_mode, mode=mode, ) # Free intermediate arrays to reduce memory before MCP - # Note: Keep trails and barriers - needed for path statistics + # Note: Keep trails, barriers, and mvum - needed for path statistics del friction_mult, friction_raw, wilderness import gc gc.collect() @@ -471,6 +489,7 @@ class OffrouteRouter: elevations = [] trail_values = [] barrier_crossings = 0 + mvum_closed_crossings = 0 for row, col in path_indices: lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) @@ -479,6 +498,8 @@ class OffrouteRouter: trail_values.append(trails[row, col]) if barriers[row, col] == 255: barrier_crossings += 1 + if mvum is not None and mvum[row, col] == 255: + mvum_closed_crossings += 1 # Calculate stats wilderness_distance_m = 0 @@ -497,8 +518,10 @@ class OffrouteRouter: total_cells = len(trail_arr) on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 - # Free trails and barriers now that path stats are computed + # Free trails, barriers, and mvum now that path stats are computed del trails, barriers + if mvum is not None: + del mvum # Entry point entry_lat = best_entry["entry_point"]["lat"] @@ -572,6 +595,7 @@ class OffrouteRouter: "on_trail_pct": on_trail_pct, "cell_count": total_cells, "barrier_crossings": barrier_crossings, + "mvum_closed_crossings": mvum_closed_crossings, "mode": mode, }, "geometry": {"type": "LineString", "coordinates": wilderness_coords} @@ -620,6 +644,7 @@ class OffrouteRouter: "network_duration_minutes": float(network_segment["duration_minutes"]) if network_segment else 0, "on_trail_pct": on_trail_pct, "barrier_crossings": barrier_crossings, + "mvum_closed_crossings": mvum_closed_crossings, "boundary_mode": boundary_mode, "mode": mode, "entry_point": { From ff0721c23ef00a978478c2708c9e0055a63be549 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 19:03:31 +0000 Subject: [PATCH 08/16] offroute: wilderness always uses foot mode for pathfinding The wilderness segment now ALWAYS uses foot mode for MCP pathfinding. The user's selected mode only affects: 1. Entry point selection (MODE_TO_VALID_HIGHWAYS filtering) 2. Valhalla costing for the network segment This ensures vehicles can navigate through wilderness (on foot) to reach roads, rather than failing when no vehicle-accessible path exists. Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 137 ++++++++++++++++++++++++----------------- 1 file changed, 81 insertions(+), 56 deletions(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index c37bddf..e1883f7 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -7,7 +7,10 @@ Entry points are extracted from OSM highways and stored in /mnt/nav/navi.db. The pathfinder routes from a wilderness start to the nearest entry point, then Valhalla completes the route to the destination. -Supports four travel modes: foot, mtb, atv, vehicle. +IMPORTANT: The wilderness segment ALWAYS uses foot mode for pathfinding. +The user's selected mode affects: + 1. Which entry points are valid (foot=any, mtb=tracks+roads, vehicle=roads only) + 2. The Valhalla costing profile for the network segment """ import json import math @@ -51,6 +54,19 @@ MODE_TO_COSTING = { "vehicle": "auto", } +# Mode to valid entry point highway classes +# foot = any trail/track/road, mtb = tracks and roads, vehicle = roads only +MODE_TO_VALID_HIGHWAYS = { + "foot": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track", "path", "footway", "bridleway"}, + "mtb": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track"}, + "atv": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track"}, + "vehicle": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service"}, +} + def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float: """Calculate distance between two points in meters.""" @@ -122,8 +138,16 @@ class EntryPointIndex: return [dict(row) for row in cur.fetchall()] - def query_radius(self, lat: float, lon: float, radius_km: float) -> List[Dict]: - """Query entry points within radius of a point.""" + def query_radius(self, lat: float, lon: float, radius_km: float, + valid_highways: Optional[set] = None) -> List[Dict]: + """ + Query entry points within radius of a point. + + Args: + lat, lon: Center point + radius_km: Search radius in kilometers + valid_highways: Optional set of valid highway classes to filter by + """ lat_delta = radius_km / 111.0 lon_delta = radius_km / (111.0 * math.cos(math.radians(lat))) @@ -134,6 +158,10 @@ class EntryPointIndex: result = [] for p in points: + # Filter by highway class if specified + if valid_highways and p['highway_class'] not in valid_highways: + continue + dist = haversine_distance(lat, lon, p['lat'], p['lon']) if dist <= radius_km * 1000: p['distance_m'] = dist @@ -262,7 +290,8 @@ class OffrouteRouter: """ OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. - Supports modes: foot, mtb, atv, vehicle + IMPORTANT: Wilderness segment ALWAYS uses foot mode for pathfinding. + User's mode affects entry point selection and Valhalla costing only. """ def __init__(self): @@ -301,9 +330,14 @@ class OffrouteRouter: Args: start_lat, start_lon: Starting coordinates (wilderness) end_lat, end_lon: Destination coordinates - mode: Travel mode (foot, mtb, atv, vehicle) + mode: Travel mode (foot, mtb, atv, vehicle) - affects entry points and network routing boundary_mode: How to handle private land (strict, pragmatic, emergency) + IMPORTANT: Wilderness pathfinding ALWAYS uses foot mode. + The user's mode only affects: + 1. Which entry points are valid targets + 2. The Valhalla costing for the network segment + Returns a GeoJSON FeatureCollection with wilderness and network segments. """ t0 = time.time() @@ -318,17 +352,29 @@ class OffrouteRouter: "message": "Trail entry point index not built. Run build_entry_index() first." } - # Find entry points near start (limit to nearest 10 to control bbox size) + # Get valid highway classes for this mode + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + + # Find entry points near start, filtered by mode MAX_ENTRY_POINTS = 10 - entry_points = self.entry_index.query_radius(start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM) + entry_points = self.entry_index.query_radius( + start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) if not entry_points: - entry_points = self.entry_index.query_radius(start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM) + # Try expanded radius + entry_points = self.entry_index.query_radius( + start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) if not entry_points: - return { - "status": "error", - "message": f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start" - } + # For non-foot modes, the error is about no suitable roads/trails + if mode == "vehicle": + msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try a different mode." + elif mode in ("mtb", "atv"): + msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try foot mode." + else: + msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start." + return {"status": "error", "message": msg} # Limit to nearest entry points to prevent huge bounding boxes entry_points = entry_points[:MAX_ENTRY_POINTS] @@ -392,15 +438,6 @@ class OffrouteRouter: target_shape=elevation.shape ) - # Load wilderness (if available and mode requires it) - wilderness = None - if self.wilderness_reader is not None and mode in ("mtb", "atv", "vehicle"): - wilderness = self.wilderness_reader.get_wilderness_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - target_shape=elevation.shape - ) - # Load trails trails = self.trail_reader.get_trails_grid( south=bbox["south"], north=bbox["north"], @@ -408,23 +445,11 @@ class OffrouteRouter: target_shape=elevation.shape ) - # Load MVUM access data (only for motorized modes) - # MVUM is motor-vehicle specific — foot mode skips entirely - mvum = None - if mode in ("mtb", "atv", "vehicle"): - try: - mvum = get_mvum_access_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - target_shape=elevation.shape, - mode=mode, - check_date=None, # TODO: accept date parameter - ) - except Exception as e: - # MVUM data may not be available - continue without it - pass + # WILDERNESS PATHFINDING ALWAYS USES FOOT MODE + # This is the key change: we don't load wilderness grid or MVUM for pathfinding + # because foot mode can traverse wilderness and doesn't need motor-vehicle access - # Compute cost grid with mode-specific parameters + # Compute cost grid with FOOT MODE (always for wilderness segment) cost = compute_cost_grid( elevation, cell_size_m=meta["cell_size_m"], @@ -432,15 +457,14 @@ class OffrouteRouter: friction_raw=friction_raw, trails=trails, barriers=barriers, - wilderness=wilderness, - mvum=mvum, + wilderness=None, # Foot mode ignores wilderness restrictions + mvum=None, # Foot mode doesn't use MVUM boundary_mode=boundary_mode, - mode=mode, + mode="foot", # ALWAYS foot for wilderness pathfinding ) # Free intermediate arrays to reduce memory before MCP - # Note: Keep trails, barriers, and mvum - needed for path statistics - del friction_mult, friction_raw, wilderness + del friction_mult, friction_raw import gc gc.collect() @@ -489,7 +513,6 @@ class OffrouteRouter: elevations = [] trail_values = [] barrier_crossings = 0 - mvum_closed_crossings = 0 for row, col in path_indices: lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) @@ -498,8 +521,6 @@ class OffrouteRouter: trail_values.append(trails[row, col]) if barriers[row, col] == 255: barrier_crossings += 1 - if mvum is not None and mvum[row, col] == 255: - mvum_closed_crossings += 1 # Calculate stats wilderness_distance_m = 0 @@ -518,10 +539,8 @@ class OffrouteRouter: total_cells = len(trail_arr) on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 - # Free trails, barriers, and mvum now that path stats are computed + # Free trails and barriers del trails, barriers - if mvum is not None: - del mvum # Entry point entry_lat = best_entry["entry_point"]["lat"] @@ -529,7 +548,7 @@ class OffrouteRouter: entry_class = best_entry["entry_point"]["highway_class"] entry_name = best_entry["entry_point"].get("name", "") - # Call Valhalla + # Call Valhalla with USER'S SELECTED MODE (not foot) valhalla_costing = MODE_TO_COSTING.get(mode, "pedestrian") valhalla_request = { @@ -595,8 +614,7 @@ class OffrouteRouter: "on_trail_pct": on_trail_pct, "cell_count": total_cells, "barrier_crossings": barrier_crossings, - "mvum_closed_crossings": mvum_closed_crossings, - "mode": mode, + "wilderness_mode": "foot", # Always foot for wilderness }, "geometry": {"type": "LineString", "coordinates": wilderness_coords} } @@ -610,6 +628,7 @@ class OffrouteRouter: "distance_km": network_segment["distance_km"], "duration_minutes": network_segment["duration_minutes"], "maneuvers": network_segment["maneuvers"], + "network_mode": mode, # User's selected mode }, "geometry": {"type": "LineString", "coordinates": network_segment["coordinates"]} } @@ -621,7 +640,12 @@ class OffrouteRouter: combined_feature = { "type": "Feature", - "properties": {"segment_type": "combined", "mode": mode, "boundary_mode": boundary_mode}, + "properties": { + "segment_type": "combined", + "wilderness_mode": "foot", + "network_mode": mode, + "boundary_mode": boundary_mode + }, "geometry": {"type": "LineString", "coordinates": combined_coords} } features.append(combined_feature) @@ -644,9 +668,9 @@ class OffrouteRouter: "network_duration_minutes": float(network_segment["duration_minutes"]) if network_segment else 0, "on_trail_pct": on_trail_pct, "barrier_crossings": barrier_crossings, - "mvum_closed_crossings": mvum_closed_crossings, "boundary_mode": boundary_mode, - "mode": mode, + "wilderness_mode": "foot", # Always foot + "network_mode": mode, # User's selection "entry_point": { "lat": entry_lat, "lon": entry_lon, "highway_class": entry_class, "name": entry_name, @@ -730,6 +754,7 @@ if __name__ == "__main__": elif len(sys.argv) > 1 and sys.argv[1] == "test": print("Testing router (all modes)...") + print("NOTE: Wilderness always uses foot mode. User mode affects entry points + network.") router = OffrouteRouter() @@ -746,8 +771,8 @@ if __name__ == "__main__": if result["status"] == "ok": s = result["summary"] - print(f" Wilderness: {s['wilderness_distance_km']:.2f} km, {s['wilderness_effort_minutes']:.1f} min") - print(f" Network: {s['network_distance_km']:.2f} km, {s['network_duration_minutes']:.1f} min") + print(f" Wilderness: {s['wilderness_distance_km']:.2f} km, {s['wilderness_effort_minutes']:.1f} min (foot)") + print(f" Network: {s['network_distance_km']:.2f} km, {s['network_duration_minutes']:.1f} min ({mode})") print(f" On-trail: {s['on_trail_pct']:.1f}%") print(f" Entry: {s['entry_point']['highway_class']}") else: From 58347415bcd36d44e39653a6ed6af541f30faef9 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 21:11:53 +0000 Subject: [PATCH 09/16] offroute: bidirectional wilderness routing (all 4 scenarios) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Support all four routing scenarios: A: off-network → on-network (wilderness then Valhalla) B: off-network → off-network (wilderness, Valhalla, wilderness) C: on-network → off-network (Valhalla then wilderness) D: on-network → on-network (pure Valhalla passthrough) Off-network detection via Valhalla /locate endpoint: - Snap distance > 500m = off-network Key implementation details: - _locate_on_network() helper for network detection - route() dispatches to scenario-specific handlers - _pathfind_wilderness() extracted for reuse (runs MCP) - _valhalla_route() helper for network segments - _build_response() unifies GeoJSON output format Memory management: - Sequential MCP runs for scenario B (not parallel) - gc.collect() after each MCP run - Bbox centered on wilderness origin, not distant destination Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 843 +++++++++++++++++++++++++++++++++-------- 1 file changed, 693 insertions(+), 150 deletions(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index e1883f7..79c9089 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -1,17 +1,20 @@ """ -OFFROUTE Router — Wilderness to network path orchestration. +OFFROUTE Router — Bidirectional wilderness-to-network path orchestration. -Connects the raster pathfinder (wilderness segment) to Valhalla (on-network segment). +Supports four routing scenarios: + A: off-network start → on-network end (wilderness then Valhalla) + B: off-network start → off-network end (wilderness, Valhalla, wilderness) + C: on-network start → off-network end (Valhalla then wilderness) + D: on-network start → on-network end (pure Valhalla passthrough) -Entry points are extracted from OSM highways and stored in /mnt/nav/navi.db. -The pathfinder routes from a wilderness start to the nearest entry point, -then Valhalla completes the route to the destination. +Off-network detection: Valhalla /locate snap distance > 500m = off-network. IMPORTANT: The wilderness segment ALWAYS uses foot mode for pathfinding. The user's selected mode affects: 1. Which entry points are valid (foot=any, mtb=tracks+roads, vehicle=roads only) 2. The Valhalla costing profile for the network segment """ +import gc import json import math import sqlite3 @@ -46,6 +49,9 @@ EXPANDED_SEARCH_RADIUS_KM = 100 # Memory limit MEMORY_LIMIT_GB = 12 +# Off-network detection threshold (meters) +OFF_NETWORK_THRESHOLD_M = 500 + # Mode to Valhalla costing mapping MODE_TO_COSTING = { "foot": "pedestrian", @@ -290,6 +296,12 @@ class OffrouteRouter: """ OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. + Supports four scenarios: + A: off-network start → on-network end + B: off-network start → off-network end + C: on-network start → off-network end + D: on-network start → on-network end (pure Valhalla) + IMPORTANT: Wilderness segment ALWAYS uses foot mode for pathfinding. User's mode affects entry point selection and Valhalla costing only. """ @@ -315,6 +327,49 @@ class OffrouteRouter: if self.trail_reader is None: self.trail_reader = TrailReader() + def _locate_on_network(self, lat: float, lon: float, mode: str) -> Dict: + """ + Check if a point is on the routable network using Valhalla's /locate. + + Returns: + { + "on_network": bool, + "snap_distance_m": float, + "snapped_lat": float, + "snapped_lon": float + } + """ + costing = MODE_TO_COSTING.get(mode, "pedestrian") + try: + resp = requests.post( + f"{VALHALLA_URL}/locate", + json={"locations": [{"lat": lat, "lon": lon}], "costing": costing}, + timeout=10 + ) + + if resp.status_code == 200: + data = resp.json() + if data and len(data) > 0 and data[0].get("edges"): + edge = data[0]["edges"][0] + snap_lat = edge.get("correlated_lat", lat) + snap_lon = edge.get("correlated_lon", lon) + snap_dist = haversine_distance(lat, lon, snap_lat, snap_lon) + return { + "on_network": snap_dist <= OFF_NETWORK_THRESHOLD_M, + "snap_distance_m": snap_dist, + "snapped_lat": snap_lat, + "snapped_lon": snap_lon + } + except Exception: + pass + + return { + "on_network": False, + "snap_distance_m": float('inf'), + "snapped_lat": lat, + "snapped_lon": lon + } + def route( self, start_lat: float, @@ -325,26 +380,167 @@ class OffrouteRouter: boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" ) -> Dict: """ - Route from a wilderness start point to a destination. + Route between two points, handling all four scenarios. + + Scenarios: + A: off-network start → on-network end (wilderness then network) + B: off-network start → off-network end (wilderness, network, wilderness) + C: on-network start → off-network end (network then wilderness) + D: on-network start → on-network end (pure network) Args: - start_lat, start_lon: Starting coordinates (wilderness) + start_lat, start_lon: Starting coordinates end_lat, end_lon: Destination coordinates - mode: Travel mode (foot, mtb, atv, vehicle) - affects entry points and network routing + mode: Travel mode (foot, mtb, atv, vehicle) boundary_mode: How to handle private land (strict, pragmatic, emergency) - IMPORTANT: Wilderness pathfinding ALWAYS uses foot mode. - The user's mode only affects: - 1. Which entry points are valid targets - 2. The Valhalla costing for the network segment - - Returns a GeoJSON FeatureCollection with wilderness and network segments. + Returns a GeoJSON FeatureCollection with route segments. """ - t0 = time.time() - if mode not in MODE_TO_COSTING: return {"status": "error", "message": f"Unknown mode: {mode}"} + # Detect network status for both endpoints + start_status = self._locate_on_network(start_lat, start_lon, mode) + end_status = self._locate_on_network(end_lat, end_lon, mode) + + start_off_network = not start_status["on_network"] + end_off_network = not end_status["on_network"] + + # Dispatch to appropriate handler + if not start_off_network and not end_off_network: + # Scenario D: on-network → on-network (pure Valhalla) + return self._route_D_network_only( + start_lat, start_lon, end_lat, end_lon, mode + ) + elif not start_off_network and end_off_network: + # Scenario C: on-network → off-network + return self._route_C_network_to_wilderness( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + elif start_off_network and not end_off_network: + # Scenario A: off-network → on-network + return self._route_A_wilderness_to_network( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + else: + # Scenario B: off-network → off-network + return self._route_B_wilderness_both( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + + def _route_D_network_only( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str + ) -> Dict: + """ + Scenario D: Both endpoints on-network. Pure Valhalla routing. + """ + t0 = time.time() + costing = MODE_TO_COSTING.get(mode, "pedestrian") + + valhalla_request = { + "locations": [ + {"lat": start_lat, "lon": start_lon}, + {"lat": end_lat, "lon": end_lon} + ], + "costing": costing, + "directions_options": {"units": "kilometers"} + } + + try: + resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) + + if resp.status_code != 200: + return { + "status": "error", + "message": f"Network routing failed: {resp.text[:200]}" + } + + valhalla_data = resp.json() + trip = valhalla_data.get("trip", {}) + legs = trip.get("legs", []) + + if not legs: + return {"status": "error", "message": "No route found"} + + leg = legs[0] + shape = leg.get("shape", "") + network_coords = self._decode_polyline(shape) + + maneuvers = [] + for m in leg.get("maneuvers", []): + maneuvers.append({ + "instruction": m.get("instruction", ""), + "type": m.get("type", 0), + "distance_km": m.get("length", 0), + "time_seconds": m.get("time", 0), + "street_names": m.get("street_names", []), + }) + + summary = trip.get("summary", {}) + distance_km = summary.get("length", 0) + duration_min = summary.get("time", 0) / 60 + + # Build response in same format as wilderness routes + network_feature = { + "type": "Feature", + "properties": { + "segment_type": "network", + "distance_km": distance_km, + "duration_minutes": duration_min, + "maneuvers": maneuvers, + "network_mode": mode, + }, + "geometry": {"type": "LineString", "coordinates": network_coords} + } + + combined_feature = { + "type": "Feature", + "properties": { + "segment_type": "combined", + "network_mode": mode, + }, + "geometry": {"type": "LineString", "coordinates": network_coords} + } + + geojson = {"type": "FeatureCollection", "features": [network_feature, combined_feature]} + + result = { + "status": "ok", + "route": geojson, + "summary": { + "total_distance_km": float(distance_km), + "total_effort_minutes": float(duration_min), + "wilderness_distance_km": 0.0, + "wilderness_effort_minutes": 0.0, + "network_distance_km": float(distance_km), + "network_duration_minutes": float(duration_min), + "on_trail_pct": 100.0, + "barrier_crossings": 0, + "network_mode": mode, + "scenario": "D", + "computation_time_s": time.time() - t0, + } + } + return result + + except Exception as e: + return {"status": "error", "message": f"Network routing failed: {e}"} + + def _route_A_wilderness_to_network( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario A: Off-network start → on-network end. + Wilderness pathfinding from start to entry point, then Valhalla to end. + """ + t0 = time.time() + # Ensure entry point index exists if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: return { @@ -362,12 +558,10 @@ class OffrouteRouter: ) if not entry_points: - # Try expanded radius entry_points = self.entry_index.query_radius( start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways ) if not entry_points: - # For non-foot modes, the error is about no suitable roads/trails if mode == "vehicle": msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try a different mode." elif mode in ("mtb", "atv"): @@ -376,13 +570,243 @@ class OffrouteRouter: msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start." return {"status": "error", "message": msg} - # Limit to nearest entry points to prevent huge bounding boxes entry_points = entry_points[:MAX_ENTRY_POINTS] - # Build bbox with max size limit (prevent OOM on large areas) - MAX_BBOX_DEGREES = 0.5 # ~55km at mid-latitudes - all_lats = [start_lat, end_lat] + [p["lat"] for p in entry_points] - all_lons = [start_lon, end_lon] + [p["lon"] for p in entry_points] + # Run wilderness pathfinding + wilderness_result = self._pathfind_wilderness( + start_lat, start_lon, end_lat, end_lon, + entry_points, boundary_mode, "start" + ) + + if wilderness_result.get("status") == "error": + return wilderness_result + + # Extract results + wilderness_coords = wilderness_result["coords"] + wilderness_stats = wilderness_result["stats"] + best_entry = wilderness_result["entry_point"] + + entry_lat = best_entry["lat"] + entry_lon = best_entry["lon"] + + # Call Valhalla from entry point to destination + network_result = self._valhalla_route(entry_lat, entry_lon, end_lat, end_lon, mode) + + # Build response + return self._build_response( + wilderness_start=wilderness_coords, + wilderness_start_stats=wilderness_stats, + network_segment=network_result.get("segment"), + wilderness_end=None, + wilderness_end_stats=None, + mode=mode, + boundary_mode=boundary_mode, + entry_start=best_entry, + entry_end=None, + scenario="A", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _route_C_network_to_wilderness( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario C: On-network start → off-network end. + Valhalla from start to entry point, then wilderness pathfinding to end. + """ + t0 = time.time() + + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + + # Find entry points near END (destination) + MAX_ENTRY_POINTS = 10 + entry_points = self.entry_index.query_radius( + end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + + if not entry_points: + entry_points = self.entry_index.query_radius( + end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points: + if mode == "vehicle": + msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try a different mode." + elif mode in ("mtb", "atv"): + msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try foot mode." + else: + msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of destination." + return {"status": "error", "message": msg} + + entry_points = entry_points[:MAX_ENTRY_POINTS] + + # 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" + ) + + if wilderness_result.get("status") == "error": + return wilderness_result + + # The path is from end→entry, reverse it for display (entry→end) + wilderness_coords = list(reversed(wilderness_result["coords"])) + wilderness_stats = wilderness_result["stats"] + best_entry = wilderness_result["entry_point"] + + entry_lat = best_entry["lat"] + entry_lon = best_entry["lon"] + + # Call Valhalla from start to entry point + network_result = self._valhalla_route(start_lat, start_lon, entry_lat, entry_lon, mode) + + # Build response (network first, then wilderness) + return self._build_response( + wilderness_start=None, + wilderness_start_stats=None, + network_segment=network_result.get("segment"), + wilderness_end=wilderness_coords, + wilderness_end_stats=wilderness_stats, + mode=mode, + boundary_mode=boundary_mode, + entry_start=None, + entry_end=best_entry, + scenario="C", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _route_B_wilderness_both( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario B: Off-network start → off-network end. + Wilderness from start to entry_A, Valhalla entry_A to entry_B, wilderness from entry_B to end. + """ + t0 = time.time() + + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + MAX_ENTRY_POINTS = 10 + + # Find entry points near START + entry_points_start = self.entry_index.query_radius( + start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_start: + entry_points_start = self.entry_index.query_radius( + start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_start: + return {"status": "error", "message": f"No entry points found near start within {EXPANDED_SEARCH_RADIUS_KM}km."} + entry_points_start = entry_points_start[:MAX_ENTRY_POINTS] + + # Find entry points near END + entry_points_end = self.entry_index.query_radius( + end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_end: + entry_points_end = self.entry_index.query_radius( + end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_end: + return {"status": "error", "message": f"No entry points found near destination within {EXPANDED_SEARCH_RADIUS_KM}km."} + entry_points_end = entry_points_end[:MAX_ENTRY_POINTS] + + # 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" + ) + + if wilderness_start_result.get("status") == "error": + return wilderness_start_result + + wilderness_start_coords = wilderness_start_result["coords"] + wilderness_start_stats = wilderness_start_result["stats"] + entry_A = wilderness_start_result["entry_point"] + + # 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" + ) + + if wilderness_end_result.get("status") == "error": + return wilderness_end_result + + # Reverse the end wilderness path (it's end→entry, we want entry→end for display) + wilderness_end_coords = list(reversed(wilderness_end_result["coords"])) + wilderness_end_stats = wilderness_end_result["stats"] + entry_B = wilderness_end_result["entry_point"] + + # Phase 3: Valhalla from entry_A to entry_B + network_result = self._valhalla_route( + entry_A["lat"], entry_A["lon"], + entry_B["lat"], entry_B["lon"], + mode + ) + + # Build response + return self._build_response( + wilderness_start=wilderness_start_coords, + wilderness_start_stats=wilderness_start_stats, + network_segment=network_result.get("segment"), + wilderness_end=wilderness_end_coords, + wilderness_end_stats=wilderness_end_stats, + mode=mode, + boundary_mode=boundary_mode, + entry_start=entry_A, + entry_end=entry_B, + scenario="B", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _pathfind_wilderness( + self, + origin_lat: float, origin_lon: float, + dest_lat: float, dest_lon: float, + entry_points: List[Dict], + boundary_mode: str, + label: str + ) -> Dict: + """ + Run MCP wilderness pathfinding from origin toward entry points. + + Args: + origin_lat, origin_lon: Starting point for pathfinding + dest_lat, dest_lon: Ultimate destination (for bbox calculation) + entry_points: List of candidate entry points + boundary_mode: How to handle barriers + label: "start" or "end" for error messages + + Returns: + {"status": "ok", "coords": [...], "stats": {...}, "entry_point": {...}} + or {"status": "error", "message": "..."} + """ + # Build bbox - only include origin and entry points, NOT distant destination + # The destination is handled by Valhalla, wilderness only needs to reach entry points + MAX_BBOX_DEGREES = 0.5 + all_lats = [origin_lat] + [p["lat"] for p in entry_points] + all_lons = [origin_lon] + [p["lon"] for p in entry_points] padding = 0.05 bbox = { @@ -392,18 +816,16 @@ class OffrouteRouter: "east": max(all_lons) + padding, } - # Clamp bbox size to prevent memory exhaustion + # Clamp bbox size, centering on origin lat_span = bbox["north"] - bbox["south"] lon_span = bbox["east"] - bbox["west"] if lat_span > MAX_BBOX_DEGREES or lon_span > MAX_BBOX_DEGREES: - center_lat = (bbox["south"] + bbox["north"]) / 2 - center_lon = (bbox["west"] + bbox["east"]) / 2 half_span = MAX_BBOX_DEGREES / 2 bbox = { - "south": center_lat - half_span, - "north": center_lat + half_span, - "west": center_lon - half_span, - "east": center_lon + half_span, + "south": origin_lat - half_span, + "north": origin_lat + half_span, + "west": origin_lon - half_span, + "east": origin_lon + half_span, } # Initialize readers @@ -416,14 +838,14 @@ class OffrouteRouter: west=bbox["west"], east=bbox["east"], ) except Exception as e: - return {"status": "error", "message": f"Failed to load elevation: {e}"} + return {"status": "error", "message": f"Failed to load elevation for {label}: {e}"} # Check memory mem = check_memory_usage() if mem > MEMORY_LIMIT_GB: return {"status": "error", "message": f"Memory limit exceeded: {mem:.1f}GB > {MEMORY_LIMIT_GB}GB"} - # Load friction (both processed and raw for mode-specific overrides) + # Load friction friction_raw = self.friction_reader.get_friction_grid( south=bbox["south"], north=bbox["north"], west=bbox["west"], east=bbox["east"], @@ -445,11 +867,7 @@ class OffrouteRouter: target_shape=elevation.shape ) - # WILDERNESS PATHFINDING ALWAYS USES FOOT MODE - # This is the key change: we don't load wilderness grid or MVUM for pathfinding - # because foot mode can traverse wilderness and doesn't need motor-vehicle access - - # Compute cost grid with FOOT MODE (always for wilderness segment) + # Compute cost grid (ALWAYS foot mode for wilderness) cost = compute_cost_grid( elevation, cell_size_m=meta["cell_size_m"], @@ -457,25 +875,24 @@ class OffrouteRouter: friction_raw=friction_raw, trails=trails, barriers=barriers, - wilderness=None, # Foot mode ignores wilderness restrictions - mvum=None, # Foot mode doesn't use MVUM + wilderness=None, + mvum=None, boundary_mode=boundary_mode, - mode="foot", # ALWAYS foot for wilderness pathfinding + mode="foot", ) - # Free intermediate arrays to reduce memory before MCP + # Free intermediate arrays del friction_mult, friction_raw - import gc gc.collect() - # Convert start to pixel coordinates - start_row, start_col = self.dem_reader.latlon_to_pixel(start_lat, start_lon, meta) + # Convert origin to pixel coordinates + origin_row, origin_col = self.dem_reader.latlon_to_pixel(origin_lat, origin_lon, meta) rows, cols = elevation.shape - if not (0 <= start_row < rows and 0 <= start_col < cols): - return {"status": "error", "message": "Start point outside grid bounds"} + if not (0 <= origin_row < rows and 0 <= origin_col < cols): + return {"status": "error", "message": f"{label.capitalize()} point outside grid bounds"} - # Mark entry points on grid + # Map entry points to pixels entry_pixels = [] for ep in entry_points: row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) @@ -483,11 +900,11 @@ class OffrouteRouter: entry_pixels.append({"row": row, "col": col, "entry_point": ep}) if not entry_pixels: - return {"status": "error", "message": "No entry points map to grid bounds"} + return {"status": "error", "message": f"No entry points map to grid bounds for {label}"} - # Run MCP pathfinder + # Run MCP mcp = MCP_Geometric(cost, fully_connected=True) - cumulative_costs, traceback = mcp.find_costs([(start_row, start_col)]) + cumulative_costs, traceback = mcp.find_costs([(origin_row, origin_col)]) # Find nearest reachable entry point best_entry = None @@ -502,67 +919,88 @@ class OffrouteRouter: if best_entry is None or np.isinf(best_cost): return { "status": "error", - "message": "No path found to any entry point (blocked by impassable terrain)" + "message": f"No path found from {label} to any entry point (blocked by impassable terrain)" } - # Traceback wilderness path + # Traceback path path_indices = mcp.traceback((best_entry["row"], best_entry["col"])) - # Convert to coordinates - wilderness_coords = [] + # Convert to coordinates and collect stats + coords = [] elevations = [] trail_values = [] barrier_crossings = 0 for row, col in path_indices: lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) - wilderness_coords.append([lon, lat]) + coords.append([lon, lat]) elevations.append(elevation[row, col]) trail_values.append(trails[row, col]) if barriers[row, col] == 255: barrier_crossings += 1 - # Calculate stats - wilderness_distance_m = 0 - for i in range(1, len(wilderness_coords)): - lon1, lat1 = wilderness_coords[i-1] - lon2, lat2 = wilderness_coords[i] - wilderness_distance_m += haversine_distance(lat1, lon1, lat2, lon2) + # Calculate distance + distance_m = 0 + for i in range(1, len(coords)): + lon1, lat1 = coords[i-1] + lon2, lat2 = coords[i] + distance_m += haversine_distance(lat1, lon1, lat2, lon2) + # Elevation stats elev_arr = np.array(elevations) elev_diff = np.diff(elev_arr) - wilderness_gain = float(np.sum(elev_diff[elev_diff > 0])) - wilderness_loss = float(np.sum(np.abs(elev_diff[elev_diff < 0]))) + elev_gain = float(np.sum(elev_diff[elev_diff > 0])) + elev_loss = float(np.sum(np.abs(elev_diff[elev_diff < 0]))) + # Trail stats trail_arr = np.array(trail_values) on_trail_cells = np.sum(trail_arr > 0) total_cells = len(trail_arr) on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 - # Free trails and barriers - del trails, barriers + # Free memory + del mcp, cumulative_costs, traceback, cost, trails, barriers, elevation + gc.collect() - # Entry point - entry_lat = best_entry["entry_point"]["lat"] - entry_lon = best_entry["entry_point"]["lon"] - entry_class = best_entry["entry_point"]["highway_class"] - entry_name = best_entry["entry_point"].get("name", "") + return { + "status": "ok", + "coords": coords, + "stats": { + "distance_km": distance_m / 1000, + "effort_minutes": best_cost / 60, + "elevation_gain_m": elev_gain, + "elevation_loss_m": elev_loss, + "on_trail_pct": on_trail_pct, + "barrier_crossings": barrier_crossings, + "cell_count": total_cells, + }, + "entry_point": best_entry["entry_point"] + } - # Call Valhalla with USER'S SELECTED MODE (not foot) - valhalla_costing = MODE_TO_COSTING.get(mode, "pedestrian") + def _valhalla_route( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str + ) -> Dict: + """ + Call Valhalla for network routing. + + Returns: + {"segment": {...}, "error": None} on success + {"segment": None, "error": "..."} on failure + """ + costing = MODE_TO_COSTING.get(mode, "pedestrian") valhalla_request = { "locations": [ - {"lat": entry_lat, "lon": entry_lon}, + {"lat": start_lat, "lon": start_lon}, {"lat": end_lat, "lon": end_lon} ], - "costing": valhalla_costing, + "costing": costing, "directions_options": {"units": "kilometers"} } - network_segment = None - valhalla_error = None - try: resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) @@ -574,7 +1012,7 @@ class OffrouteRouter: if legs: leg = legs[0] shape = leg.get("shape", "") - network_coords = self._decode_polyline(shape) + coords = self._decode_polyline(shape) maneuvers = [] for m in leg.get("maneuvers", []): @@ -587,97 +1025,184 @@ class OffrouteRouter: }) summary = trip.get("summary", {}) - network_segment = { - "coordinates": network_coords, - "distance_km": summary.get("length", 0), - "duration_minutes": summary.get("time", 0) / 60, - "maneuvers": maneuvers, + return { + "segment": { + "coordinates": coords, + "distance_km": summary.get("length", 0), + "duration_minutes": summary.get("time", 0) / 60, + "maneuvers": maneuvers, + }, + "error": None } - else: - valhalla_error = f"Valhalla returned {resp.status_code}: {resp.text[:200]}" + + return {"segment": None, "error": f"Valhalla returned {resp.status_code}: {resp.text[:200]}"} except Exception as e: - valhalla_error = f"Valhalla request failed: {e}" + return {"segment": None, "error": f"Valhalla request failed: {e}"} - # Build response + def _build_response( + self, + wilderness_start: Optional[List], + wilderness_start_stats: Optional[Dict], + network_segment: Optional[Dict], + wilderness_end: Optional[List], + wilderness_end_stats: Optional[Dict], + mode: str, + boundary_mode: str, + entry_start: Optional[Dict], + entry_end: Optional[Dict], + scenario: str, + t0: float, + valhalla_error: Optional[str] + ) -> Dict: + """Build the final GeoJSON response.""" features = [] - wilderness_feature = { - "type": "Feature", - "properties": { - "segment_type": "wilderness", - "effort_minutes": float(best_cost / 60), - "distance_km": float(wilderness_distance_m / 1000), - "elevation_gain_m": wilderness_gain, - "elevation_loss_m": wilderness_loss, - "boundary_mode": boundary_mode, - "on_trail_pct": on_trail_pct, - "cell_count": total_cells, - "barrier_crossings": barrier_crossings, - "wilderness_mode": "foot", # Always foot for wilderness - }, - "geometry": {"type": "LineString", "coordinates": wilderness_coords} - } - features.append(wilderness_feature) + # Wilderness start segment + if wilderness_start and wilderness_start_stats: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "wilderness", + "segment_position": "start", + "effort_minutes": float(wilderness_start_stats["effort_minutes"]), + "distance_km": float(wilderness_start_stats["distance_km"]), + "elevation_gain_m": wilderness_start_stats["elevation_gain_m"], + "elevation_loss_m": wilderness_start_stats["elevation_loss_m"], + "boundary_mode": boundary_mode, + "on_trail_pct": wilderness_start_stats["on_trail_pct"], + "barrier_crossings": wilderness_start_stats["barrier_crossings"], + "wilderness_mode": "foot", + }, + "geometry": {"type": "LineString", "coordinates": wilderness_start} + }) + # Network segment if network_segment: - network_feature = { + features.append({ "type": "Feature", "properties": { "segment_type": "network", "distance_km": network_segment["distance_km"], "duration_minutes": network_segment["duration_minutes"], "maneuvers": network_segment["maneuvers"], - "network_mode": mode, # User's selected mode + "network_mode": mode, }, "geometry": {"type": "LineString", "coordinates": network_segment["coordinates"]} - } - features.append(network_feature) + }) - combined_coords = wilderness_coords.copy() + # Wilderness end segment + if wilderness_end and wilderness_end_stats: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "wilderness", + "segment_position": "end", + "effort_minutes": float(wilderness_end_stats["effort_minutes"]), + "distance_km": float(wilderness_end_stats["distance_km"]), + "elevation_gain_m": wilderness_end_stats["elevation_gain_m"], + "elevation_loss_m": wilderness_end_stats["elevation_loss_m"], + "boundary_mode": boundary_mode, + "on_trail_pct": wilderness_end_stats["on_trail_pct"], + "barrier_crossings": wilderness_end_stats["barrier_crossings"], + "wilderness_mode": "foot", + }, + "geometry": {"type": "LineString", "coordinates": wilderness_end} + }) + + # Combined path + combined_coords = [] + if wilderness_start: + combined_coords.extend(wilderness_start) if network_segment: - combined_coords.extend(network_segment["coordinates"][1:]) + # Skip first coord if we already have wilderness_start (avoid duplicate) + start_idx = 1 if wilderness_start else 0 + combined_coords.extend(network_segment["coordinates"][start_idx:]) + if wilderness_end: + # Skip first coord (avoid duplicate with network end) + start_idx = 1 if (wilderness_start or network_segment) else 0 + combined_coords.extend(wilderness_end[start_idx:]) - combined_feature = { - "type": "Feature", - "properties": { - "segment_type": "combined", - "wilderness_mode": "foot", - "network_mode": mode, - "boundary_mode": boundary_mode - }, - "geometry": {"type": "LineString", "coordinates": combined_coords} - } - features.append(combined_feature) + if combined_coords: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "combined", + "wilderness_mode": "foot", + "network_mode": mode, + "boundary_mode": boundary_mode, + "scenario": scenario, + }, + "geometry": {"type": "LineString", "coordinates": combined_coords} + }) geojson = {"type": "FeatureCollection", "features": features} - total_distance_km = wilderness_distance_m / 1000 - total_effort_minutes = best_cost / 60 + # Calculate totals + total_distance_km = 0.0 + total_effort_minutes = 0.0 + wilderness_distance_km = 0.0 + wilderness_effort_minutes = 0.0 + network_distance_km = 0.0 + network_duration_minutes = 0.0 + barrier_crossings = 0 + on_trail_pct = 0.0 + + if wilderness_start_stats: + wilderness_distance_km += wilderness_start_stats["distance_km"] + wilderness_effort_minutes += wilderness_start_stats["effort_minutes"] + barrier_crossings += wilderness_start_stats["barrier_crossings"] + on_trail_pct = wilderness_start_stats["on_trail_pct"] + + if wilderness_end_stats: + wilderness_distance_km += wilderness_end_stats["distance_km"] + wilderness_effort_minutes += wilderness_end_stats["effort_minutes"] + barrier_crossings += wilderness_end_stats["barrier_crossings"] + # Average on-trail percentage if we have both + if wilderness_start_stats: + on_trail_pct = (on_trail_pct + wilderness_end_stats["on_trail_pct"]) / 2 + else: + on_trail_pct = wilderness_end_stats["on_trail_pct"] if network_segment: - total_distance_km += network_segment["distance_km"] - total_effort_minutes += network_segment["duration_minutes"] + network_distance_km = network_segment["distance_km"] + network_duration_minutes = network_segment["duration_minutes"] + + total_distance_km = wilderness_distance_km + network_distance_km + total_effort_minutes = wilderness_effort_minutes + network_duration_minutes summary = { "total_distance_km": float(total_distance_km), "total_effort_minutes": float(total_effort_minutes), - "wilderness_distance_km": float(wilderness_distance_m / 1000), - "wilderness_effort_minutes": float(best_cost / 60), - "network_distance_km": float(network_segment["distance_km"]) if network_segment else 0, - "network_duration_minutes": float(network_segment["duration_minutes"]) if network_segment else 0, - "on_trail_pct": on_trail_pct, + "wilderness_distance_km": float(wilderness_distance_km), + "wilderness_effort_minutes": float(wilderness_effort_minutes), + "network_distance_km": float(network_distance_km), + "network_duration_minutes": float(network_duration_minutes), + "on_trail_pct": float(on_trail_pct), "barrier_crossings": barrier_crossings, "boundary_mode": boundary_mode, - "wilderness_mode": "foot", # Always foot - "network_mode": mode, # User's selection - "entry_point": { - "lat": entry_lat, "lon": entry_lon, - "highway_class": entry_class, "name": entry_name, - }, + "wilderness_mode": "foot", + "network_mode": mode, + "scenario": scenario, "computation_time_s": time.time() - t0, } + if entry_start: + summary["entry_point_start"] = { + "lat": entry_start["lat"], + "lon": entry_start["lon"], + "highway_class": entry_start["highway_class"], + "name": entry_start.get("name", ""), + } + + if entry_end: + summary["entry_point_end"] = { + "lat": entry_end["lat"], + "lon": entry_end["lon"], + "highway_class": entry_end["highway_class"], + "name": entry_end.get("name", ""), + } + result = {"status": "ok", "route": geojson, "summary": summary} if valhalla_error: @@ -753,28 +1278,46 @@ if __name__ == "__main__": print(f"\nDone. Total entry points: {stats['total']}") elif len(sys.argv) > 1 and sys.argv[1] == "test": - print("Testing router (all modes)...") - print("NOTE: Wilderness always uses foot mode. User mode affects entry points + network.") + print("Testing router (all scenarios)...") + print("=" * 60) router = OffrouteRouter() - for mode in ["foot", "mtb", "atv", "vehicle"]: - print(f"\n{'='*60}") - print(f"Mode: {mode}") - print("="*60) + # Test points + wilderness_start = (44.0543, -115.4237) # Off-network + wilderness_end = (45.2, -115.5) # Deep wilderness (Frank Church) + road_start = (43.6150, -116.2023) # Boise downtown (on-network) + road_end = (43.5867, -116.5625) # Nampa (on-network) + + tests = [ + ("A: wilderness→road", wilderness_start, (44.0814, -115.5021)), + ("B: wilderness→wilderness", wilderness_start, wilderness_end), + ("C: road→wilderness", road_start, wilderness_start), + ("D: road→road", road_start, road_end), + ] + + for label, (slat, slon), (elat, elon) in tests: + print(f"\n{label}") + print("-" * 40) result = router.route( - start_lat=42.35, start_lon=-114.30, - end_lat=42.5629, end_lon=-114.4609, - mode=mode, boundary_mode="pragmatic" + start_lat=slat, start_lon=slon, + end_lat=elat, end_lon=elon, + mode="foot", boundary_mode="pragmatic" ) if result["status"] == "ok": s = result["summary"] - print(f" Wilderness: {s['wilderness_distance_km']:.2f} km, {s['wilderness_effort_minutes']:.1f} min (foot)") - print(f" Network: {s['network_distance_km']:.2f} km, {s['network_duration_minutes']:.1f} min ({mode})") - print(f" On-trail: {s['on_trail_pct']:.1f}%") - print(f" Entry: {s['entry_point']['highway_class']}") + print(f" Scenario: {s.get('scenario', '?')}") + print(f" Total: {s['total_distance_km']:.2f} km, {s['total_effort_minutes']:.1f} min") + print(f" Wilderness: {s['wilderness_distance_km']:.2f} km") + print(f" Network: {s['network_distance_km']:.2f} km") + if s.get('entry_point_start'): + ep = s['entry_point_start'] + print(f" Entry (start): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") + if s.get('entry_point_end'): + ep = s['entry_point_end'] + print(f" Entry (end): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") else: print(f" ERROR: {result['message']}") @@ -783,4 +1326,4 @@ if __name__ == "__main__": else: print("Usage:") print(" python router.py build # Build entry point index") - print(" python router.py test # Test all modes") + print(" python router.py test # Test all scenarios") From 87a4741b8d4ebecfc276256d7f73d4481bd8a216 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 21:19:04 +0000 Subject: [PATCH 10/16] =?UTF-8?q?offroute:=20raise=20bbox=20limit=20to=202?= =?UTF-8?q?.0=C2=B0=20(~220km=20coverage)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index 79c9089..ccc956d 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -804,7 +804,7 @@ class OffrouteRouter: """ # Build bbox - only include origin and entry points, NOT distant destination # The destination is handled by Valhalla, wilderness only needs to reach entry points - MAX_BBOX_DEGREES = 0.5 + MAX_BBOX_DEGREES = 2.0 all_lats = [origin_lat] + [p["lat"] for p in entry_points] all_lons = [origin_lon] + [p["lon"] for p in entry_points] From cf758476b4d44be559664a2e92211566dd7d92aa Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 21:55:31 +0000 Subject: [PATCH 11/16] offroute: add auto mode for standard driving routes Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index ccc956d..6981e89 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -53,7 +53,8 @@ MEMORY_LIMIT_GB = 12 OFF_NETWORK_THRESHOLD_M = 500 # Mode to Valhalla costing mapping -MODE_TO_COSTING = { +MODE_TO_COSTING = { + "auto": "auto", "foot": "pedestrian", "mtb": "bicycle", "atv": "auto", @@ -62,7 +63,9 @@ MODE_TO_COSTING = { # Mode to valid entry point highway classes # foot = any trail/track/road, mtb = tracks and roads, vehicle = roads only -MODE_TO_VALID_HIGHWAYS = { +MODE_TO_VALID_HIGHWAYS = { + "auto": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service"}, "foot": {"primary", "secondary", "tertiary", "unclassified", "residential", "service", "track", "path", "footway", "bridleway"}, "mtb": {"primary", "secondary", "tertiary", "unclassified", "residential", From 686b35710a55b369d8eafe6f554f58e795a36289 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 22:37:49 +0000 Subject: [PATCH 12/16] api: add auto mode to offroute endpoint validation Co-Authored-By: Claude Opus 4.5 --- lib/api.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/api.py b/lib/api.py index 699d09d..949a0cc 100644 --- a/lib/api.py +++ b/lib/api.py @@ -2768,8 +2768,8 @@ def api_offroute(): # Parse options mode = data.get("mode", "foot") - if mode not in ("foot", "mtb", "atv", "vehicle"): - return jsonify({"status": "error", "message": "mode must be foot, mtb, atv, or vehicle"}), 400 + if mode not in ("auto", "foot", "mtb", "atv", "vehicle"): + return jsonify({"status": "error", "message": "mode must be auto, foot, mtb, atv, or vehicle"}), 400 boundary_mode = data.get("boundary_mode", "pragmatic") if boundary_mode not in ("strict", "pragmatic", "emergency"): From 05c24f95f635bc856894f8f869448716042124ab Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 8 May 2026 23:27:06 +0000 Subject: [PATCH 13/16] offroute: tighten off-network threshold to 10m Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index 6981e89..0cc3ccd 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -50,7 +50,7 @@ EXPANDED_SEARCH_RADIUS_KM = 100 MEMORY_LIMIT_GB = 12 # Off-network detection threshold (meters) -OFF_NETWORK_THRESHOLD_M = 500 +OFF_NETWORK_THRESHOLD_M = 10 # Mode to Valhalla costing mapping MODE_TO_COSTING = { From b4e33eb0484c99bcf53d76e71a92a1d3dd43ab40 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 9 May 2026 03:28:58 +0000 Subject: [PATCH 14/16] offroute: PostGIS entry points with 100m densification and land_status tagging - Migrate EntryPointIndex from SQLite to PostGIS (padus database) - Densify highway LineStrings at 100m intervals via Shapely interpolate - 2.94M entry points from 476k lines (4x more coverage) - Tag each entry point with land_status via ST_Intersects against padus_sub - 1.64M public (56%), 1.30M unknown (44%) - Add geography GIST index for fast radius queries (~25ms) - Increase OFF_NETWORK_THRESHOLD_M from 10m to 50m for GPS accuracy - PBF path and PostGIS DSN configurable via home.yaml Co-Authored-By: Claude Opus 4.5 --- config/profiles/home.yaml | 17 +- lib/offroute/router.py | 2799 +++++++++++++++++++------------------ 2 files changed, 1483 insertions(+), 1333 deletions(-) diff --git a/config/profiles/home.yaml b/config/profiles/home.yaml index 5269812..474ffb2 100644 --- a/config/profiles/home.yaml +++ b/config/profiles/home.yaml @@ -6,13 +6,13 @@ profile: home region_name: "North America" tileset: - url: "/tiles/na.pmtiles" + url: "/tiles/planet/current.pmtiles" bounds: [-168, 14, -52, 72] max_zoom: 15 attribution: "Protomaps © OSM" tileset_hillshade: - url: "/tiles/hillshade-na.pmtiles" + url: "/tiles/planet-dem.pmtiles" encoding: "terrarium" max_zoom: 12 @@ -33,14 +33,14 @@ services: features: has_nominatim_details: true - has_kiwix_wiki: false + has_kiwix_wiki: true has_hillshade: true has_3d_terrain: false has_traffic_overlay: true has_landclass: true has_public_lands_layer: true has_contours: true - has_contours_test: true + has_contours_test: false has_contours_test_10ft: false has_address_book_write: false has_overture_enrichment: true @@ -48,7 +48,16 @@ features: has_contacts: true has_wiki_rewriting: true has_wiki_discovery: false + has_usfs_trails: true + has_blm_trails: true defaults: center: [42.5736, -114.6066] zoom: 10 + +# Offroute wilderness routing +offroute: + osm_pbf_path: "/mnt/nav/sources/idaho-latest.osm.pbf" + densify_interval_m: 100 + postgis_dsn: "dbname=padus" + diff --git a/lib/offroute/router.py b/lib/offroute/router.py index 0cc3ccd..4b988ab 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -1,1332 +1,1473 @@ -""" -OFFROUTE Router — Bidirectional wilderness-to-network path orchestration. - -Supports four routing scenarios: - A: off-network start → on-network end (wilderness then Valhalla) - B: off-network start → off-network end (wilderness, Valhalla, wilderness) - C: on-network start → off-network end (Valhalla then wilderness) - D: on-network start → on-network end (pure Valhalla passthrough) - -Off-network detection: Valhalla /locate snap distance > 500m = off-network. - -IMPORTANT: The wilderness segment ALWAYS uses foot mode for pathfinding. -The user's selected mode affects: - 1. Which entry points are valid (foot=any, mtb=tracks+roads, vehicle=roads only) - 2. The Valhalla costing profile for the network segment -""" -import gc -import json -import math -import sqlite3 -import subprocess -import tempfile -import time -from pathlib import Path -from typing import Dict, List, Optional, Tuple, Literal - -import numpy as np -import requests -from skimage.graph import MCP_Geometric - -from .dem import DEMReader -from .cost import compute_cost_grid -from .friction import FrictionReader, friction_to_multiplier -from .barriers import BarrierReader, WildernessReader, DEFAULT_WILDERNESS_PATH -from .trails import TrailReader -from .mvum import get_mvum_access_grid - -# Paths -NAVI_DB_PATH = Path("/mnt/nav/navi.db") -OSM_PBF_PATH = Path("/mnt/nav/sources/idaho-latest.osm.pbf") - -# Valhalla endpoint -VALHALLA_URL = "http://localhost:8002" - -# Search radius for entry points (km) -DEFAULT_SEARCH_RADIUS_KM = 50 -EXPANDED_SEARCH_RADIUS_KM = 100 - -# Memory limit -MEMORY_LIMIT_GB = 12 - -# Off-network detection threshold (meters) -OFF_NETWORK_THRESHOLD_M = 10 - -# Mode to Valhalla costing mapping +""" +OFFROUTE Router — Bidirectional wilderness-to-network path orchestration. + +Supports four routing scenarios: + A: off-network start → on-network end (wilderness then Valhalla) + B: off-network start → off-network end (wilderness, Valhalla, wilderness) + C: on-network start → off-network end (Valhalla then wilderness) + D: on-network start → on-network end (pure Valhalla passthrough) + +Off-network detection: Valhalla /locate snap distance > 500m = off-network. + +IMPORTANT: The wilderness segment ALWAYS uses foot mode for pathfinding. +The user's selected mode affects: + 1. Which entry points are valid (foot=any, mtb=tracks+roads, vehicle=roads only) + 2. The Valhalla costing profile for the network segment +""" +import gc +import json +import math +import subprocess +import tempfile +import time +from pathlib import Path +from typing import Dict, List, Optional, Tuple, Literal, Set + +import numpy as np +import requests +import psycopg2 +import psycopg2.extras +from shapely.geometry import LineString +from skimage.graph import MCP_Geometric + +from .dem import DEMReader +from .cost import compute_cost_grid +from .friction import FrictionReader, friction_to_multiplier +from .barriers import BarrierReader, WildernessReader, DEFAULT_WILDERNESS_PATH +from .trails import TrailReader +from .mvum import get_mvum_access_grid +from ..deployment_config import get_deployment_config + +# Load configuration +_deploy_config = get_deployment_config() +_offroute_config = _deploy_config.get("offroute", {}) + +# Paths (configurable via home.yaml) +OSM_PBF_PATH = Path(_offroute_config.get("osm_pbf_path", "/mnt/nav/sources/idaho-latest.osm.pbf")) +DENSIFY_INTERVAL_M = _offroute_config.get("densify_interval_m", 100) +POSTGIS_DSN = _offroute_config.get("postgis_dsn", "dbname=padus user=postgres") + +# Legacy SQLite path (still used by MVUM) +NAVI_DB_PATH = Path("/mnt/nav/navi.db") + +# Valhalla endpoint +VALHALLA_URL = "http://localhost:8002" + +# Search radius for entry points (km) +DEFAULT_SEARCH_RADIUS_KM = 50 +EXPANDED_SEARCH_RADIUS_KM = 100 + +# Memory limit +MEMORY_LIMIT_GB = 12 + +# Off-network detection threshold (meters) +OFF_NETWORK_THRESHOLD_M = 50 + +# Mode to Valhalla costing mapping MODE_TO_COSTING = { - "auto": "auto", - "foot": "pedestrian", - "mtb": "bicycle", - "atv": "auto", - "vehicle": "auto", -} - -# Mode to valid entry point highway classes -# foot = any trail/track/road, mtb = tracks and roads, vehicle = roads only + "auto": "auto", + "foot": "pedestrian", + "mtb": "bicycle", + "atv": "auto", + "vehicle": "auto", +} + +# Mode to valid entry point highway classes +# foot = any trail/track/road, mtb = tracks and roads, vehicle = roads only MODE_TO_VALID_HIGHWAYS = { "auto": {"primary", "secondary", "tertiary", "unclassified", "residential", - "service"}, - "foot": {"primary", "secondary", "tertiary", "unclassified", "residential", - "service", "track", "path", "footway", "bridleway"}, - "mtb": {"primary", "secondary", "tertiary", "unclassified", "residential", - "service", "track"}, - "atv": {"primary", "secondary", "tertiary", "unclassified", "residential", - "service", "track"}, - "vehicle": {"primary", "secondary", "tertiary", "unclassified", "residential", - "service"}, -} - - -def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float: - """Calculate distance between two points in meters.""" - R = 6371000 - dlat = math.radians(lat2 - lat1) - dlon = math.radians(lon2 - lon1) - a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2 - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) - return R * c - - -def check_memory_usage() -> float: - """Check current memory usage in GB.""" - try: - import psutil - process = psutil.Process() - return process.memory_info().rss / (1024**3) - except ImportError: - return 0 - - -class EntryPointIndex: - """ - Trail entry point index for wilderness-to-network handoff. - - Entry points are endpoints and intersections of OSM highways - that connect wilderness areas to the routable network. - """ - - def __init__(self, db_path: Path = NAVI_DB_PATH): - self.db_path = db_path - self._conn = None - - def _get_conn(self) -> sqlite3.Connection: - if self._conn is None: - self._conn = sqlite3.connect(str(self.db_path)) - self._conn.row_factory = sqlite3.Row - return self._conn - - def table_exists(self) -> bool: - """Check if trail_entry_points table exists.""" - if not self.db_path.exists(): - return False - conn = self._get_conn() - cur = conn.execute( - "SELECT name FROM sqlite_master WHERE type='table' AND name='trail_entry_points'" - ) - return cur.fetchone() is not None - - def get_entry_point_count(self) -> int: - """Get count of entry points.""" - if not self.table_exists(): - return 0 - conn = self._get_conn() - cur = conn.execute("SELECT COUNT(*) FROM trail_entry_points") - return cur.fetchone()[0] - - def query_bbox(self, south: float, north: float, west: float, east: float) -> List[Dict]: - """Query entry points within a bounding box.""" - if not self.table_exists(): - return [] - - conn = self._get_conn() - cur = conn.execute(""" - SELECT id, lat, lon, highway_class, name - FROM trail_entry_points - WHERE lat >= ? AND lat <= ? AND lon >= ? AND lon <= ? - """, (south, north, west, east)) - - return [dict(row) for row in cur.fetchall()] - - def query_radius(self, lat: float, lon: float, radius_km: float, - valid_highways: Optional[set] = None) -> List[Dict]: - """ - Query entry points within radius of a point. - - Args: - lat, lon: Center point - radius_km: Search radius in kilometers - valid_highways: Optional set of valid highway classes to filter by - """ - lat_delta = radius_km / 111.0 - lon_delta = radius_km / (111.0 * math.cos(math.radians(lat))) - - points = self.query_bbox( - lat - lat_delta, lat + lat_delta, - lon - lon_delta, lon + lon_delta - ) - - result = [] - for p in points: - # Filter by highway class if specified - if valid_highways and p['highway_class'] not in valid_highways: - continue - - dist = haversine_distance(lat, lon, p['lat'], p['lon']) - if dist <= radius_km * 1000: - p['distance_m'] = dist - result.append(p) - - return sorted(result, key=lambda x: x['distance_m']) - - def build_index(self, osm_pbf_path: Path = OSM_PBF_PATH) -> Dict: - """Build the entry point index from OSM PBF.""" - if not osm_pbf_path.exists(): - raise FileNotFoundError(f"OSM PBF not found: {osm_pbf_path}") - - print(f"Building trail entry point index from {osm_pbf_path}...") - - highway_types = [ - "primary", "secondary", "tertiary", "unclassified", - "residential", "service", "track", "path", "footway", "bridleway" - ] - - stats = {"total": 0, "by_class": {}} - - with tempfile.TemporaryDirectory() as tmpdir: - geojson_path = Path(tmpdir) / "highways.geojson" - - print(f" Extracting highways with osmium...") - cmd = ["osmium", "tags-filter", str(osm_pbf_path)] - for ht in highway_types: - cmd.append(f"w/highway={ht}") - cmd.extend(["-o", str(Path(tmpdir) / "filtered.osm.pbf"), "--overwrite"]) - subprocess.run(cmd, check=True, capture_output=True) - - print(f" Converting to GeoJSON with ogr2ogr...") - cmd = [ - "ogr2ogr", "-f", "GeoJSON", - str(geojson_path), - str(Path(tmpdir) / "filtered.osm.pbf"), - "lines", "-t_srs", "EPSG:4326" - ] - subprocess.run(cmd, check=True, capture_output=True) - - print(f" Extracting entry points...") - with open(geojson_path) as f: - data = json.load(f) - - points = {} - for feature in data.get("features", []): - props = feature.get("properties", {}) - geom = feature.get("geometry", {}) - - if geom.get("type") != "LineString": - continue - - coords = geom.get("coordinates", []) - if len(coords) < 2: - continue - - highway_class = props.get("highway", "unknown") - name = props.get("name", "") - - for coord in [coords[0], coords[-1]]: - lon, lat = coord[0], coord[1] - key = (round(lat, 5), round(lon, 5)) - - if key not in points: - points[key] = { - "lat": lat, "lon": lon, - "highway_class": highway_class, "name": name - } - else: - existing = points[key] - if self._highway_priority(highway_class) < self._highway_priority(existing["highway_class"]): - points[key]["highway_class"] = highway_class - if name and not existing["name"]: - points[key]["name"] = name - - print(f" Writing {len(points)} entry points to {self.db_path}...") - - self.db_path.parent.mkdir(parents=True, exist_ok=True) - conn = self._get_conn() - - conn.execute(""" - CREATE TABLE IF NOT EXISTS trail_entry_points ( - id INTEGER PRIMARY KEY AUTOINCREMENT, - lat REAL NOT NULL, lon REAL NOT NULL, - highway_class TEXT NOT NULL, name TEXT - ) - """) - conn.execute("DELETE FROM trail_entry_points") - - for point in points.values(): - conn.execute( - "INSERT INTO trail_entry_points (lat, lon, highway_class, name) VALUES (?, ?, ?, ?)", - (point["lat"], point["lon"], point["highway_class"], point["name"]) - ) - stats["total"] += 1 - hc = point["highway_class"] - stats["by_class"][hc] = stats["by_class"].get(hc, 0) + 1 - - conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lat ON trail_entry_points(lat)") - conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_lon ON trail_entry_points(lon)") - conn.execute("CREATE INDEX IF NOT EXISTS idx_entry_latlon ON trail_entry_points(lat, lon)") - conn.commit() - - print(f" Done. Total: {stats['total']} entry points") - for hc, count in sorted(stats["by_class"].items(), key=lambda x: -x[1]): - print(f" {hc}: {count}") - - return stats - - def _highway_priority(self, highway_class: str) -> int: - """Lower number = better priority for entry points.""" - priority = { - "primary": 1, "secondary": 2, "tertiary": 3, - "unclassified": 4, "residential": 5, "service": 6, - "track": 7, "path": 8, "footway": 9, "bridleway": 10 - } - return priority.get(highway_class, 99) - - def close(self): - if self._conn: - self._conn.close() - self._conn = None - - -class OffrouteRouter: - """ - OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. - - Supports four scenarios: - A: off-network start → on-network end - B: off-network start → off-network end - C: on-network start → off-network end - D: on-network start → on-network end (pure Valhalla) - - IMPORTANT: Wilderness segment ALWAYS uses foot mode for pathfinding. - User's mode affects entry point selection and Valhalla costing only. - """ - - def __init__(self): - self.dem_reader = None - self.friction_reader = None - self.barrier_reader = None - self.wilderness_reader = None - self.trail_reader = None - self.entry_index = EntryPointIndex() - - def _init_readers(self): - """Lazy init readers.""" - if self.dem_reader is None: - self.dem_reader = DEMReader() - if self.friction_reader is None: - self.friction_reader = FrictionReader() - if self.barrier_reader is None: - self.barrier_reader = BarrierReader() - if self.wilderness_reader is None and DEFAULT_WILDERNESS_PATH.exists(): - self.wilderness_reader = WildernessReader() - if self.trail_reader is None: - self.trail_reader = TrailReader() - - def _locate_on_network(self, lat: float, lon: float, mode: str) -> Dict: - """ - Check if a point is on the routable network using Valhalla's /locate. - - Returns: - { - "on_network": bool, - "snap_distance_m": float, - "snapped_lat": float, - "snapped_lon": float - } - """ - costing = MODE_TO_COSTING.get(mode, "pedestrian") - try: - resp = requests.post( - f"{VALHALLA_URL}/locate", - json={"locations": [{"lat": lat, "lon": lon}], "costing": costing}, - timeout=10 - ) - - if resp.status_code == 200: - data = resp.json() - if data and len(data) > 0 and data[0].get("edges"): - edge = data[0]["edges"][0] - snap_lat = edge.get("correlated_lat", lat) - snap_lon = edge.get("correlated_lon", lon) - snap_dist = haversine_distance(lat, lon, snap_lat, snap_lon) - return { - "on_network": snap_dist <= OFF_NETWORK_THRESHOLD_M, - "snap_distance_m": snap_dist, - "snapped_lat": snap_lat, - "snapped_lon": snap_lon - } - except Exception: - pass - - return { - "on_network": False, - "snap_distance_m": float('inf'), - "snapped_lat": lat, - "snapped_lon": lon - } - - def route( - self, - start_lat: float, - start_lon: float, - end_lat: float, - end_lon: float, - mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot", - boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" - ) -> Dict: - """ - Route between two points, handling all four scenarios. - - Scenarios: - A: off-network start → on-network end (wilderness then network) - B: off-network start → off-network end (wilderness, network, wilderness) - C: on-network start → off-network end (network then wilderness) - D: on-network start → on-network end (pure network) - - Args: - start_lat, start_lon: Starting coordinates - end_lat, end_lon: Destination coordinates - mode: Travel mode (foot, mtb, atv, vehicle) - boundary_mode: How to handle private land (strict, pragmatic, emergency) - - Returns a GeoJSON FeatureCollection with route segments. - """ - if mode not in MODE_TO_COSTING: - return {"status": "error", "message": f"Unknown mode: {mode}"} - - # Detect network status for both endpoints - start_status = self._locate_on_network(start_lat, start_lon, mode) - end_status = self._locate_on_network(end_lat, end_lon, mode) - - start_off_network = not start_status["on_network"] - end_off_network = not end_status["on_network"] - - # Dispatch to appropriate handler - if not start_off_network and not end_off_network: - # Scenario D: on-network → on-network (pure Valhalla) - return self._route_D_network_only( - start_lat, start_lon, end_lat, end_lon, mode - ) - elif not start_off_network and end_off_network: - # Scenario C: on-network → off-network - return self._route_C_network_to_wilderness( - start_lat, start_lon, end_lat, end_lon, mode, boundary_mode - ) - elif start_off_network and not end_off_network: - # Scenario A: off-network → on-network - return self._route_A_wilderness_to_network( - start_lat, start_lon, end_lat, end_lon, mode, boundary_mode - ) - else: - # Scenario B: off-network → off-network - return self._route_B_wilderness_both( - start_lat, start_lon, end_lat, end_lon, mode, boundary_mode - ) - - def _route_D_network_only( - self, - start_lat: float, start_lon: float, - end_lat: float, end_lon: float, - mode: str - ) -> Dict: - """ - Scenario D: Both endpoints on-network. Pure Valhalla routing. - """ - t0 = time.time() - costing = MODE_TO_COSTING.get(mode, "pedestrian") - - valhalla_request = { - "locations": [ - {"lat": start_lat, "lon": start_lon}, - {"lat": end_lat, "lon": end_lon} - ], - "costing": costing, - "directions_options": {"units": "kilometers"} - } - - try: - resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) - - if resp.status_code != 200: - return { - "status": "error", - "message": f"Network routing failed: {resp.text[:200]}" - } - - valhalla_data = resp.json() - trip = valhalla_data.get("trip", {}) - legs = trip.get("legs", []) - - if not legs: - return {"status": "error", "message": "No route found"} - - leg = legs[0] - shape = leg.get("shape", "") - network_coords = self._decode_polyline(shape) - - maneuvers = [] - for m in leg.get("maneuvers", []): - maneuvers.append({ - "instruction": m.get("instruction", ""), - "type": m.get("type", 0), - "distance_km": m.get("length", 0), - "time_seconds": m.get("time", 0), - "street_names": m.get("street_names", []), - }) - - summary = trip.get("summary", {}) - distance_km = summary.get("length", 0) - duration_min = summary.get("time", 0) / 60 - - # Build response in same format as wilderness routes - network_feature = { - "type": "Feature", - "properties": { - "segment_type": "network", - "distance_km": distance_km, - "duration_minutes": duration_min, - "maneuvers": maneuvers, - "network_mode": mode, - }, - "geometry": {"type": "LineString", "coordinates": network_coords} - } - - combined_feature = { - "type": "Feature", - "properties": { - "segment_type": "combined", - "network_mode": mode, - }, - "geometry": {"type": "LineString", "coordinates": network_coords} - } - - geojson = {"type": "FeatureCollection", "features": [network_feature, combined_feature]} - - result = { - "status": "ok", - "route": geojson, - "summary": { - "total_distance_km": float(distance_km), - "total_effort_minutes": float(duration_min), - "wilderness_distance_km": 0.0, - "wilderness_effort_minutes": 0.0, - "network_distance_km": float(distance_km), - "network_duration_minutes": float(duration_min), - "on_trail_pct": 100.0, - "barrier_crossings": 0, - "network_mode": mode, - "scenario": "D", - "computation_time_s": time.time() - t0, - } - } - return result - - except Exception as e: - return {"status": "error", "message": f"Network routing failed: {e}"} - - def _route_A_wilderness_to_network( - self, - start_lat: float, start_lon: float, - end_lat: float, end_lon: float, - mode: str, boundary_mode: str - ) -> Dict: - """ - Scenario A: Off-network start → on-network end. - Wilderness pathfinding from start to entry point, then Valhalla to end. - """ - t0 = time.time() - - # Ensure entry point index exists - if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: - return { - "status": "error", - "message": "Trail entry point index not built. Run build_entry_index() first." - } - - # Get valid highway classes for this mode - valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) - - # Find entry points near start, filtered by mode - MAX_ENTRY_POINTS = 10 - entry_points = self.entry_index.query_radius( - start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways - ) - - if not entry_points: - entry_points = self.entry_index.query_radius( - start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points: - if mode == "vehicle": - msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try a different mode." - elif mode in ("mtb", "atv"): - msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try foot mode." - else: - msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start." - return {"status": "error", "message": msg} - - entry_points = entry_points[:MAX_ENTRY_POINTS] - - # Run wilderness pathfinding - wilderness_result = self._pathfind_wilderness( - start_lat, start_lon, end_lat, end_lon, - entry_points, boundary_mode, "start" - ) - - if wilderness_result.get("status") == "error": - return wilderness_result - - # Extract results - wilderness_coords = wilderness_result["coords"] - wilderness_stats = wilderness_result["stats"] - best_entry = wilderness_result["entry_point"] - - entry_lat = best_entry["lat"] - entry_lon = best_entry["lon"] - - # Call Valhalla from entry point to destination - network_result = self._valhalla_route(entry_lat, entry_lon, end_lat, end_lon, mode) - - # Build response - return self._build_response( - wilderness_start=wilderness_coords, - wilderness_start_stats=wilderness_stats, - network_segment=network_result.get("segment"), - wilderness_end=None, - wilderness_end_stats=None, - mode=mode, - boundary_mode=boundary_mode, - entry_start=best_entry, - entry_end=None, - scenario="A", - t0=t0, - valhalla_error=network_result.get("error") - ) - - def _route_C_network_to_wilderness( - self, - start_lat: float, start_lon: float, - end_lat: float, end_lon: float, - mode: str, boundary_mode: str - ) -> Dict: - """ - Scenario C: On-network start → off-network end. - Valhalla from start to entry point, then wilderness pathfinding to end. - """ - t0 = time.time() - - if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: - return { - "status": "error", - "message": "Trail entry point index not built. Run build_entry_index() first." - } - - valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) - - # Find entry points near END (destination) - MAX_ENTRY_POINTS = 10 - entry_points = self.entry_index.query_radius( - end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways - ) - - if not entry_points: - entry_points = self.entry_index.query_radius( - end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points: - if mode == "vehicle": - msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try a different mode." - elif mode in ("mtb", "atv"): - msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try foot mode." - else: - msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of destination." - return {"status": "error", "message": msg} - - entry_points = entry_points[:MAX_ENTRY_POINTS] - - # 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" - ) - - if wilderness_result.get("status") == "error": - return wilderness_result - - # The path is from end→entry, reverse it for display (entry→end) - wilderness_coords = list(reversed(wilderness_result["coords"])) - wilderness_stats = wilderness_result["stats"] - best_entry = wilderness_result["entry_point"] - - entry_lat = best_entry["lat"] - entry_lon = best_entry["lon"] - - # Call Valhalla from start to entry point - network_result = self._valhalla_route(start_lat, start_lon, entry_lat, entry_lon, mode) - - # Build response (network first, then wilderness) - return self._build_response( - wilderness_start=None, - wilderness_start_stats=None, - network_segment=network_result.get("segment"), - wilderness_end=wilderness_coords, - wilderness_end_stats=wilderness_stats, - mode=mode, - boundary_mode=boundary_mode, - entry_start=None, - entry_end=best_entry, - scenario="C", - t0=t0, - valhalla_error=network_result.get("error") - ) - - def _route_B_wilderness_both( - self, - start_lat: float, start_lon: float, - end_lat: float, end_lon: float, - mode: str, boundary_mode: str - ) -> Dict: - """ - Scenario B: Off-network start → off-network end. - Wilderness from start to entry_A, Valhalla entry_A to entry_B, wilderness from entry_B to end. - """ - t0 = time.time() - - if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: - return { - "status": "error", - "message": "Trail entry point index not built. Run build_entry_index() first." - } - - valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) - MAX_ENTRY_POINTS = 10 - - # Find entry points near START - entry_points_start = self.entry_index.query_radius( - start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points_start: - entry_points_start = self.entry_index.query_radius( - start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points_start: - return {"status": "error", "message": f"No entry points found near start within {EXPANDED_SEARCH_RADIUS_KM}km."} - entry_points_start = entry_points_start[:MAX_ENTRY_POINTS] - - # Find entry points near END - entry_points_end = self.entry_index.query_radius( - end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points_end: - entry_points_end = self.entry_index.query_radius( - end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways - ) - if not entry_points_end: - return {"status": "error", "message": f"No entry points found near destination within {EXPANDED_SEARCH_RADIUS_KM}km."} - entry_points_end = entry_points_end[:MAX_ENTRY_POINTS] - - # 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" - ) - - if wilderness_start_result.get("status") == "error": - return wilderness_start_result - - wilderness_start_coords = wilderness_start_result["coords"] - wilderness_start_stats = wilderness_start_result["stats"] - entry_A = wilderness_start_result["entry_point"] - - # 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" - ) - - if wilderness_end_result.get("status") == "error": - return wilderness_end_result - - # Reverse the end wilderness path (it's end→entry, we want entry→end for display) - wilderness_end_coords = list(reversed(wilderness_end_result["coords"])) - wilderness_end_stats = wilderness_end_result["stats"] - entry_B = wilderness_end_result["entry_point"] - - # Phase 3: Valhalla from entry_A to entry_B - network_result = self._valhalla_route( - entry_A["lat"], entry_A["lon"], - entry_B["lat"], entry_B["lon"], - mode - ) - - # Build response - return self._build_response( - wilderness_start=wilderness_start_coords, - wilderness_start_stats=wilderness_start_stats, - network_segment=network_result.get("segment"), - wilderness_end=wilderness_end_coords, - wilderness_end_stats=wilderness_end_stats, - mode=mode, - boundary_mode=boundary_mode, - entry_start=entry_A, - entry_end=entry_B, - scenario="B", - t0=t0, - valhalla_error=network_result.get("error") - ) - - def _pathfind_wilderness( - self, - origin_lat: float, origin_lon: float, - dest_lat: float, dest_lon: float, - entry_points: List[Dict], - boundary_mode: str, - label: str - ) -> Dict: - """ - Run MCP wilderness pathfinding from origin toward entry points. - - Args: - origin_lat, origin_lon: Starting point for pathfinding - dest_lat, dest_lon: Ultimate destination (for bbox calculation) - entry_points: List of candidate entry points - boundary_mode: How to handle barriers - label: "start" or "end" for error messages - - Returns: - {"status": "ok", "coords": [...], "stats": {...}, "entry_point": {...}} - or {"status": "error", "message": "..."} - """ - # Build bbox - only include origin and entry points, NOT distant destination - # The destination is handled by Valhalla, wilderness only needs to reach entry points - MAX_BBOX_DEGREES = 2.0 - all_lats = [origin_lat] + [p["lat"] for p in entry_points] - all_lons = [origin_lon] + [p["lon"] for p in entry_points] - - padding = 0.05 - bbox = { - "south": min(all_lats) - padding, - "north": max(all_lats) + padding, - "west": min(all_lons) - padding, - "east": max(all_lons) + padding, - } - - # Clamp bbox size, centering on origin - lat_span = bbox["north"] - bbox["south"] - lon_span = bbox["east"] - bbox["west"] - if lat_span > MAX_BBOX_DEGREES or lon_span > MAX_BBOX_DEGREES: - half_span = MAX_BBOX_DEGREES / 2 - bbox = { - "south": origin_lat - half_span, - "north": origin_lat + half_span, - "west": origin_lon - half_span, - "east": origin_lon + half_span, - } - - # Initialize readers - self._init_readers() - - # Load elevation - try: - elevation, meta = self.dem_reader.get_elevation_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - ) - except Exception as e: - return {"status": "error", "message": f"Failed to load elevation for {label}: {e}"} - - # Check memory - mem = check_memory_usage() - if mem > MEMORY_LIMIT_GB: - return {"status": "error", "message": f"Memory limit exceeded: {mem:.1f}GB > {MEMORY_LIMIT_GB}GB"} - - # Load friction - friction_raw = self.friction_reader.get_friction_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - target_shape=elevation.shape - ) - friction_mult = friction_to_multiplier(friction_raw) - - # Load barriers - barriers = self.barrier_reader.get_barrier_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - target_shape=elevation.shape - ) - - # Load trails - trails = self.trail_reader.get_trails_grid( - south=bbox["south"], north=bbox["north"], - west=bbox["west"], east=bbox["east"], - target_shape=elevation.shape - ) - - # Compute cost grid (ALWAYS foot mode for wilderness) - cost = compute_cost_grid( - elevation, - cell_size_m=meta["cell_size_m"], - friction=friction_mult, - friction_raw=friction_raw, - trails=trails, - barriers=barriers, - wilderness=None, - mvum=None, - boundary_mode=boundary_mode, - mode="foot", - ) - - # Free intermediate arrays - del friction_mult, friction_raw - gc.collect() - - # Convert origin to pixel coordinates - origin_row, origin_col = self.dem_reader.latlon_to_pixel(origin_lat, origin_lon, meta) - - rows, cols = elevation.shape - 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 - entry_pixels = [] - for ep in entry_points: - row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) - if 0 <= row < rows and 0 <= col < cols: - entry_pixels.append({"row": row, "col": col, "entry_point": ep}) - - 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)]) - - # Find nearest reachable entry point - best_entry = None - best_cost = np.inf - - 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): - 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"])) - - # Convert to coordinates and collect stats - coords = [] - elevations = [] - trail_values = [] - barrier_crossings = 0 - - for row, col in path_indices: - lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) - coords.append([lon, lat]) - elevations.append(elevation[row, col]) - trail_values.append(trails[row, col]) - if barriers[row, col] == 255: - barrier_crossings += 1 - - # Calculate distance - distance_m = 0 - for i in range(1, len(coords)): - lon1, lat1 = coords[i-1] - lon2, lat2 = coords[i] - distance_m += haversine_distance(lat1, lon1, lat2, lon2) - - # Elevation stats - elev_arr = np.array(elevations) - elev_diff = np.diff(elev_arr) - elev_gain = float(np.sum(elev_diff[elev_diff > 0])) - elev_loss = float(np.sum(np.abs(elev_diff[elev_diff < 0]))) - - # Trail stats - trail_arr = np.array(trail_values) - on_trail_cells = np.sum(trail_arr > 0) - total_cells = len(trail_arr) - 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 - gc.collect() - - return { - "status": "ok", - "coords": coords, - "stats": { - "distance_km": distance_m / 1000, - "effort_minutes": best_cost / 60, - "elevation_gain_m": elev_gain, - "elevation_loss_m": elev_loss, - "on_trail_pct": on_trail_pct, - "barrier_crossings": barrier_crossings, - "cell_count": total_cells, - }, - "entry_point": best_entry["entry_point"] - } - - def _valhalla_route( - self, - start_lat: float, start_lon: float, - end_lat: float, end_lon: float, - mode: str - ) -> Dict: - """ - Call Valhalla for network routing. - - Returns: - {"segment": {...}, "error": None} on success - {"segment": None, "error": "..."} on failure - """ - costing = MODE_TO_COSTING.get(mode, "pedestrian") - - valhalla_request = { - "locations": [ - {"lat": start_lat, "lon": start_lon}, - {"lat": end_lat, "lon": end_lon} - ], - "costing": costing, - "directions_options": {"units": "kilometers"} - } - - try: - resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) - - if resp.status_code == 200: - valhalla_data = resp.json() - trip = valhalla_data.get("trip", {}) - legs = trip.get("legs", []) - - if legs: - leg = legs[0] - shape = leg.get("shape", "") - coords = self._decode_polyline(shape) - - maneuvers = [] - for m in leg.get("maneuvers", []): - maneuvers.append({ - "instruction": m.get("instruction", ""), - "type": m.get("type", 0), - "distance_km": m.get("length", 0), - "time_seconds": m.get("time", 0), - "street_names": m.get("street_names", []), - }) - - summary = trip.get("summary", {}) - return { - "segment": { - "coordinates": coords, - "distance_km": summary.get("length", 0), - "duration_minutes": summary.get("time", 0) / 60, - "maneuvers": maneuvers, - }, - "error": None - } - - return {"segment": None, "error": f"Valhalla returned {resp.status_code}: {resp.text[:200]}"} - - except Exception as e: - return {"segment": None, "error": f"Valhalla request failed: {e}"} - - def _build_response( - self, - wilderness_start: Optional[List], - wilderness_start_stats: Optional[Dict], - network_segment: Optional[Dict], - wilderness_end: Optional[List], - wilderness_end_stats: Optional[Dict], - mode: str, - boundary_mode: str, - entry_start: Optional[Dict], - entry_end: Optional[Dict], - scenario: str, - t0: float, - valhalla_error: Optional[str] - ) -> Dict: - """Build the final GeoJSON response.""" - features = [] - - # Wilderness start segment - if wilderness_start and wilderness_start_stats: - features.append({ - "type": "Feature", - "properties": { - "segment_type": "wilderness", - "segment_position": "start", - "effort_minutes": float(wilderness_start_stats["effort_minutes"]), - "distance_km": float(wilderness_start_stats["distance_km"]), - "elevation_gain_m": wilderness_start_stats["elevation_gain_m"], - "elevation_loss_m": wilderness_start_stats["elevation_loss_m"], - "boundary_mode": boundary_mode, - "on_trail_pct": wilderness_start_stats["on_trail_pct"], - "barrier_crossings": wilderness_start_stats["barrier_crossings"], - "wilderness_mode": "foot", - }, - "geometry": {"type": "LineString", "coordinates": wilderness_start} - }) - - # Network segment - if network_segment: - features.append({ - "type": "Feature", - "properties": { - "segment_type": "network", - "distance_km": network_segment["distance_km"], - "duration_minutes": network_segment["duration_minutes"], - "maneuvers": network_segment["maneuvers"], - "network_mode": mode, - }, - "geometry": {"type": "LineString", "coordinates": network_segment["coordinates"]} - }) - - # Wilderness end segment - if wilderness_end and wilderness_end_stats: - features.append({ - "type": "Feature", - "properties": { - "segment_type": "wilderness", - "segment_position": "end", - "effort_minutes": float(wilderness_end_stats["effort_minutes"]), - "distance_km": float(wilderness_end_stats["distance_km"]), - "elevation_gain_m": wilderness_end_stats["elevation_gain_m"], - "elevation_loss_m": wilderness_end_stats["elevation_loss_m"], - "boundary_mode": boundary_mode, - "on_trail_pct": wilderness_end_stats["on_trail_pct"], - "barrier_crossings": wilderness_end_stats["barrier_crossings"], - "wilderness_mode": "foot", - }, - "geometry": {"type": "LineString", "coordinates": wilderness_end} - }) - - # Combined path - combined_coords = [] - if wilderness_start: - combined_coords.extend(wilderness_start) - if network_segment: - # Skip first coord if we already have wilderness_start (avoid duplicate) - start_idx = 1 if wilderness_start else 0 - combined_coords.extend(network_segment["coordinates"][start_idx:]) - if wilderness_end: - # Skip first coord (avoid duplicate with network end) - start_idx = 1 if (wilderness_start or network_segment) else 0 - combined_coords.extend(wilderness_end[start_idx:]) - - if combined_coords: - features.append({ - "type": "Feature", - "properties": { - "segment_type": "combined", - "wilderness_mode": "foot", - "network_mode": mode, - "boundary_mode": boundary_mode, - "scenario": scenario, - }, - "geometry": {"type": "LineString", "coordinates": combined_coords} - }) - - geojson = {"type": "FeatureCollection", "features": features} - - # Calculate totals - total_distance_km = 0.0 - total_effort_minutes = 0.0 - wilderness_distance_km = 0.0 - wilderness_effort_minutes = 0.0 - network_distance_km = 0.0 - network_duration_minutes = 0.0 - barrier_crossings = 0 - on_trail_pct = 0.0 - - if wilderness_start_stats: - wilderness_distance_km += wilderness_start_stats["distance_km"] - wilderness_effort_minutes += wilderness_start_stats["effort_minutes"] - barrier_crossings += wilderness_start_stats["barrier_crossings"] - on_trail_pct = wilderness_start_stats["on_trail_pct"] - - if wilderness_end_stats: - wilderness_distance_km += wilderness_end_stats["distance_km"] - wilderness_effort_minutes += wilderness_end_stats["effort_minutes"] - barrier_crossings += wilderness_end_stats["barrier_crossings"] - # Average on-trail percentage if we have both - if wilderness_start_stats: - on_trail_pct = (on_trail_pct + wilderness_end_stats["on_trail_pct"]) / 2 - else: - on_trail_pct = wilderness_end_stats["on_trail_pct"] - - if network_segment: - network_distance_km = network_segment["distance_km"] - network_duration_minutes = network_segment["duration_minutes"] - - total_distance_km = wilderness_distance_km + network_distance_km - total_effort_minutes = wilderness_effort_minutes + network_duration_minutes - - summary = { - "total_distance_km": float(total_distance_km), - "total_effort_minutes": float(total_effort_minutes), - "wilderness_distance_km": float(wilderness_distance_km), - "wilderness_effort_minutes": float(wilderness_effort_minutes), - "network_distance_km": float(network_distance_km), - "network_duration_minutes": float(network_duration_minutes), - "on_trail_pct": float(on_trail_pct), - "barrier_crossings": barrier_crossings, - "boundary_mode": boundary_mode, - "wilderness_mode": "foot", - "network_mode": mode, - "scenario": scenario, - "computation_time_s": time.time() - t0, - } - - if entry_start: - summary["entry_point_start"] = { - "lat": entry_start["lat"], - "lon": entry_start["lon"], - "highway_class": entry_start["highway_class"], - "name": entry_start.get("name", ""), - } - - if entry_end: - summary["entry_point_end"] = { - "lat": entry_end["lat"], - "lon": entry_end["lon"], - "highway_class": entry_end["highway_class"], - "name": entry_end.get("name", ""), - } - - result = {"status": "ok", "route": geojson, "summary": summary} - - if valhalla_error: - result["warning"] = f"Network segment incomplete: {valhalla_error}" - - return result - - def _decode_polyline(self, encoded: str, precision: int = 6) -> List[List[float]]: - """Decode a polyline string into coordinates [lon, lat].""" - coords = [] - index = 0 - lat = 0 - lon = 0 - - while index < len(encoded): - shift = 0 - result = 0 - while True: - b = ord(encoded[index]) - 63 - index += 1 - result |= (b & 0x1f) << shift - shift += 5 - if b < 0x20: - break - dlat = ~(result >> 1) if result & 1 else result >> 1 - lat += dlat - - shift = 0 - result = 0 - while True: - b = ord(encoded[index]) - 63 - index += 1 - result |= (b & 0x1f) << shift - shift += 5 - if b < 0x20: - break - dlon = ~(result >> 1) if result & 1 else result >> 1 - lon += dlon - - coords.append([lon / (10 ** precision), lat / (10 ** precision)]) - - return coords - - def close(self): - """Close all readers.""" - if self.dem_reader: - self.dem_reader.close() - if self.friction_reader: - self.friction_reader.close() - if self.barrier_reader: - self.barrier_reader.close() - if self.wilderness_reader: - self.wilderness_reader.close() - if self.trail_reader: - self.trail_reader.close() - self.entry_index.close() - - -def build_entry_index(): - """Build the trail entry point index.""" - index = EntryPointIndex() - stats = index.build_index() - index.close() - return stats - - -if __name__ == "__main__": - import sys - - if len(sys.argv) > 1 and sys.argv[1] == "build": - print("Building trail entry point index...") - stats = build_entry_index() - print(f"\nDone. Total entry points: {stats['total']}") - - elif len(sys.argv) > 1 and sys.argv[1] == "test": - print("Testing router (all scenarios)...") - print("=" * 60) - - router = OffrouteRouter() - - # Test points - wilderness_start = (44.0543, -115.4237) # Off-network - wilderness_end = (45.2, -115.5) # Deep wilderness (Frank Church) - road_start = (43.6150, -116.2023) # Boise downtown (on-network) - road_end = (43.5867, -116.5625) # Nampa (on-network) - - tests = [ - ("A: wilderness→road", wilderness_start, (44.0814, -115.5021)), - ("B: wilderness→wilderness", wilderness_start, wilderness_end), - ("C: road→wilderness", road_start, wilderness_start), - ("D: road→road", road_start, road_end), - ] - - for label, (slat, slon), (elat, elon) in tests: - print(f"\n{label}") - print("-" * 40) - - result = router.route( - start_lat=slat, start_lon=slon, - end_lat=elat, end_lon=elon, - mode="foot", boundary_mode="pragmatic" - ) - - if result["status"] == "ok": - s = result["summary"] - print(f" Scenario: {s.get('scenario', '?')}") - print(f" Total: {s['total_distance_km']:.2f} km, {s['total_effort_minutes']:.1f} min") - print(f" Wilderness: {s['wilderness_distance_km']:.2f} km") - print(f" Network: {s['network_distance_km']:.2f} km") - if s.get('entry_point_start'): - ep = s['entry_point_start'] - print(f" Entry (start): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") - if s.get('entry_point_end'): - ep = s['entry_point_end'] - print(f" Entry (end): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") - else: - print(f" ERROR: {result['message']}") - - router.close() - - else: - print("Usage:") - print(" python router.py build # Build entry point index") - print(" python router.py test # Test all scenarios") + "service"}, + "foot": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track", "path", "footway", "bridleway"}, + "mtb": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track"}, + "atv": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service", "track"}, + "vehicle": {"primary", "secondary", "tertiary", "unclassified", "residential", + "service"}, +} + + +def haversine_distance(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """Calculate distance between two points in meters.""" + R = 6371000 + dlat = math.radians(lat2 - lat1) + dlon = math.radians(lon2 - lon1) + a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2 + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + return R * c + + +def check_memory_usage() -> float: + """Check current memory usage in GB.""" + try: + import psutil + process = psutil.Process() + return process.memory_info().rss / (1024**3) + except ImportError: + return 0 + + +class EntryPointIndex: + """ + PostGIS-backed spatial index of road/trail entry points. + Uses ST_DWithin for fast radius queries with meter-accurate distances. + Densifies highway LineStrings at 100m intervals for better coverage. + """ + + def __init__(self, dsn: str = None): + self.dsn = dsn or POSTGIS_DSN + self._conn: Optional[psycopg2.extensions.connection] = None + + def _get_conn(self) -> psycopg2.extensions.connection: + if self._conn is None or self._conn.closed: + self._conn = psycopg2.connect(self.dsn) + return self._conn + + def table_exists(self) -> bool: + """Check if entry_points table exists.""" + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute(""" + SELECT EXISTS ( + SELECT FROM information_schema.tables + WHERE table_name = 'entry_points' + ) + """) + return cur.fetchone()[0] + + def get_entry_point_count(self) -> int: + """Return the number of entry points in the index.""" + if not self.table_exists(): + return 0 + conn = self._get_conn() + with conn.cursor() as cur: + cur.execute("SELECT COUNT(*) FROM entry_points") + return cur.fetchone()[0] + + def query_bbox( + self, + south: float, + north: float, + west: float, + east: float, + valid_highways: Optional[Set[str]] = None + ) -> List[Dict]: + """Find entry points within a bounding box.""" + if not self.table_exists(): + return [] + + conn = self._get_conn() + + highway_filter = "" + params = [west, south, east, north] + if valid_highways: + placeholders = ','.join(['%s'] * len(valid_highways)) + highway_filter = f"AND highway_class IN ({placeholders})" + params.extend(list(valid_highways)) + + query = f""" + SELECT + id, + ST_Y(geom) as lat, + ST_X(geom) as lon, + highway_class, + name, + land_status + FROM entry_points + WHERE geom && ST_MakeEnvelope(%s, %s, %s, %s, 4326) + {highway_filter} + """ + + with conn.cursor(cursor_factory=psycopg2.extras.RealDictCursor) as cur: + cur.execute(query, params) + return [dict(row) for row in cur.fetchall()] + + def query_radius( + self, + lat: float, + lon: float, + radius_km: float, + valid_highways: Optional[Set[str]] = None, + limit: int = 50 + ) -> List[Dict]: + """ + Find entry points within radius_km of (lat, lon). + Uses PostGIS ST_DWithin with geography cast for meter-accurate distance. + """ + if not self.table_exists(): + return [] + + conn = self._get_conn() + radius_m = radius_km * 1000 + + # Build query with optional highway filter + highway_filter = "" + params = [lon, lat, lon, lat, radius_m] + if valid_highways: + placeholders = ','.join(['%s'] * len(valid_highways)) + highway_filter = f"AND highway_class IN ({placeholders})" + params.extend(list(valid_highways)) + params.append(limit) + + query = f""" + SELECT + id, + ST_Y(geom) as lat, + ST_X(geom) as lon, + highway_class, + name, + land_status, + ST_Distance( + geom::geography, + ST_SetSRID(ST_Point(%s, %s), 4326)::geography + ) as distance_m + FROM entry_points + WHERE ST_DWithin( + geom::geography, + ST_SetSRID(ST_Point(%s, %s), 4326)::geography, + %s + ) + {highway_filter} + ORDER BY distance_m + LIMIT %s + """ + + with conn.cursor(cursor_factory=psycopg2.extras.RealDictCursor) as cur: + cur.execute(query, params) + return [dict(row) for row in cur.fetchall()] + + def build_index(self, osm_pbf_path: Path = None) -> Dict: + """ + Build the entry point index from OSM PBF. + Densifies LineStrings to sample points every 100m. + Tags points with land_status from PAD-US. + """ + if osm_pbf_path is None: + osm_pbf_path = OSM_PBF_PATH + + if not osm_pbf_path.exists(): + raise FileNotFoundError(f"OSM PBF not found: {osm_pbf_path}") + + print(f"Building entry point index from {osm_pbf_path}...") + start_time = time.time() + + highway_types = [ + "primary", "secondary", "tertiary", "unclassified", + "residential", "service", "track", "path", "footway", "bridleway" + ] + + stats = {"total": 0, "by_class": {}, "lines_processed": 0} + + with tempfile.TemporaryDirectory() as tmpdir: + geojson_path = Path(tmpdir) / "highways.geojson" + + # Extract highways with osmium + print(" Extracting highways with osmium...") + cmd = ["osmium", "tags-filter", str(osm_pbf_path)] + for ht in highway_types: + cmd.append(f"w/highway={ht}") + cmd.extend(["-o", str(Path(tmpdir) / "filtered.osm.pbf"), "--overwrite"]) + subprocess.run(cmd, check=True, capture_output=True) + + # Convert to GeoJSON + print(" Converting to GeoJSON with ogr2ogr...") + cmd = [ + "ogr2ogr", "-f", "GeoJSON", + str(geojson_path), + str(Path(tmpdir) / "filtered.osm.pbf"), + "lines", "-t_srs", "EPSG:4326" + ] + subprocess.run(cmd, check=True, capture_output=True) + + # Load GeoJSON + print(" Loading GeoJSON...") + with open(geojson_path) as f: + data = json.load(f) + + # Process features and densify + print(f" Densifying LineStrings at {DENSIFY_INTERVAL_M}m intervals...") + points_to_insert = [] + seen_keys = set() + + features = data.get("features", []) + total_features = len(features) + + for idx, feature in enumerate(features): + if idx > 0 and idx % 100000 == 0: + print(f" Processed {idx}/{total_features} features...") + + props = feature.get("properties", {}) + geom = feature.get("geometry", {}) + + if geom.get("type") != "LineString": + continue + + coords = geom.get("coordinates", []) + if len(coords) < 2: + continue + + highway_class = props.get("highway", "unknown") + name = props.get("name", "") + stats["lines_processed"] += 1 + + # Densify this LineString + densified = self._densify_line(coords, DENSIFY_INTERVAL_M) + + for lon, lat in densified: + # Deduplicate by rounding to 5 decimal places (~1m precision) + key = (round(lat, 5), round(lon, 5)) + if key in seen_keys: + continue + seen_keys.add(key) + + points_to_insert.append((lon, lat, highway_class, name)) + + # Insert into PostGIS + print(f" Inserting {len(points_to_insert)} entry points into PostGIS...") + conn = self._get_conn() + + with conn.cursor() as cur: + # Truncate existing data + cur.execute("TRUNCATE entry_points RESTART IDENTITY") + + # Batch insert with execute_values for speed + batch_size = 50000 + for i in range(0, len(points_to_insert), batch_size): + batch = points_to_insert[i:i+batch_size] + psycopg2.extras.execute_values( + cur, + """ + INSERT INTO entry_points (geom, highway_class, name) + VALUES %s + """, + batch, + template="(ST_SetSRID(ST_Point(%s, %s), 4326), %s, %s)", + page_size=10000 + ) + if i > 0 and i % 500000 == 0: + print(f" Inserted {i}/{len(points_to_insert)} points...") + + conn.commit() + + # Tag land_status from PAD-US + print(" Tagging land_status from PAD-US subdivided polygons...") + with conn.cursor() as cur: + cur.execute(""" + UPDATE entry_points e + SET land_status = 'public' + FROM padus_sub p + WHERE ST_Intersects(e.geom, p.geom) + """) + public_count = cur.rowcount + print(f" Tagged {public_count} points as public land") + + conn.commit() + + # Gather stats + elapsed = time.time() - start_time + stats["total"] = len(points_to_insert) + stats["build_time_sec"] = round(elapsed, 1) + + for lon, lat, hc, name in points_to_insert: + stats["by_class"][hc] = stats["by_class"].get(hc, 0) + 1 + + print(f" Done in {elapsed:.1f}s. Total: {stats['total']} entry points from {stats['lines_processed']} lines") + for hc, count in sorted(stats["by_class"].items(), key=lambda x: -x[1]): + print(f" {hc}: {count}") + + return stats + + def _densify_line(self, coords: List[List[float]], interval_m: float) -> List[tuple]: + """ + Sample points along a LineString at regular intervals. + coords: [[lon, lat], ...] in GeoJSON order + Returns: [(lon, lat), ...] sampled points including first and last + """ + if len(coords) < 2: + return [(coords[0][0], coords[0][1])] if coords else [] + + # Calculate line length in meters using haversine on segments + total_m = 0 + for i in range(len(coords) - 1): + lon1, lat1 = coords[i] + lon2, lat2 = coords[i + 1] + total_m += haversine_distance(lat1, lon1, lat2, lon2) + + if total_m == 0: + return [(coords[0][0], coords[0][1])] + + # Create Shapely LineString + line = LineString(coords) + + # Calculate number of points needed + n_points = max(2, int(total_m / interval_m) + 1) + + # Sample using normalized interpolation + result = [] + for i in range(n_points): + fraction = min(i / (n_points - 1), 1.0) if n_points > 1 else 0 + point = line.interpolate(fraction, normalized=True) + result.append((point.x, point.y)) # (lon, lat) + + # Always ensure first and last original coordinates are included + first_coord = (coords[0][0], coords[0][1]) + last_coord = (coords[-1][0], coords[-1][1]) + + if result[0] != first_coord: + result[0] = first_coord + if result[-1] != last_coord: + result[-1] = last_coord + + return result + + def _highway_priority(self, highway_class: str) -> int: + """Lower number = better priority for entry points.""" + priority = { + "primary": 1, "secondary": 2, "tertiary": 3, + "unclassified": 4, "residential": 5, "service": 6, + "track": 7, "path": 8, "footway": 9, "bridleway": 10 + } + return priority.get(highway_class, 99) + + def close(self): + if self._conn and not self._conn.closed: + self._conn.close() + self._conn = None + + +class OffrouteRouter: + """ + OFFROUTE Router — orchestrates wilderness pathfinding and Valhalla stitching. + + Supports four scenarios: + A: off-network start → on-network end + B: off-network start → off-network end + C: on-network start → off-network end + D: on-network start → on-network end (pure Valhalla) + + IMPORTANT: Wilderness segment ALWAYS uses foot mode for pathfinding. + User's mode affects entry point selection and Valhalla costing only. + """ + + def __init__(self): + self.dem_reader = None + self.friction_reader = None + self.barrier_reader = None + self.wilderness_reader = None + self.trail_reader = None + self.entry_index = EntryPointIndex() + + def _init_readers(self): + """Lazy init readers.""" + if self.dem_reader is None: + self.dem_reader = DEMReader() + if self.friction_reader is None: + self.friction_reader = FrictionReader() + if self.barrier_reader is None: + self.barrier_reader = BarrierReader() + if self.wilderness_reader is None and DEFAULT_WILDERNESS_PATH.exists(): + self.wilderness_reader = WildernessReader() + if self.trail_reader is None: + self.trail_reader = TrailReader() + + def _locate_on_network(self, lat: float, lon: float, mode: str) -> Dict: + """ + Check if a point is on the routable network using Valhalla's /locate. + + Returns: + { + "on_network": bool, + "snap_distance_m": float, + "snapped_lat": float, + "snapped_lon": float + } + """ + costing = MODE_TO_COSTING.get(mode, "pedestrian") + try: + resp = requests.post( + f"{VALHALLA_URL}/locate", + json={"locations": [{"lat": lat, "lon": lon}], "costing": costing}, + timeout=10 + ) + + if resp.status_code == 200: + data = resp.json() + if data and len(data) > 0 and data[0].get("edges"): + edge = data[0]["edges"][0] + snap_lat = edge.get("correlated_lat", lat) + snap_lon = edge.get("correlated_lon", lon) + snap_dist = haversine_distance(lat, lon, snap_lat, snap_lon) + return { + "on_network": snap_dist <= OFF_NETWORK_THRESHOLD_M, + "snap_distance_m": snap_dist, + "snapped_lat": snap_lat, + "snapped_lon": snap_lon + } + except Exception: + pass + + return { + "on_network": False, + "snap_distance_m": float('inf'), + "snapped_lat": lat, + "snapped_lon": lon + } + + def route( + self, + start_lat: float, + start_lon: float, + end_lat: float, + end_lon: float, + mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot", + boundary_mode: Literal["strict", "pragmatic", "emergency"] = "pragmatic" + ) -> Dict: + """ + Route between two points, handling all four scenarios. + + Scenarios: + A: off-network start → on-network end (wilderness then network) + B: off-network start → off-network end (wilderness, network, wilderness) + C: on-network start → off-network end (network then wilderness) + D: on-network start → on-network end (pure network) + + Args: + start_lat, start_lon: Starting coordinates + end_lat, end_lon: Destination coordinates + mode: Travel mode (foot, mtb, atv, vehicle) + boundary_mode: How to handle private land (strict, pragmatic, emergency) + + Returns a GeoJSON FeatureCollection with route segments. + """ + if mode not in MODE_TO_COSTING: + return {"status": "error", "message": f"Unknown mode: {mode}"} + + # Detect network status for both endpoints + start_status = self._locate_on_network(start_lat, start_lon, mode) + end_status = self._locate_on_network(end_lat, end_lon, mode) + + start_off_network = not start_status["on_network"] + end_off_network = not end_status["on_network"] + + # Dispatch to appropriate handler + if not start_off_network and not end_off_network: + # Scenario D: on-network → on-network (pure Valhalla) + return self._route_D_network_only( + start_lat, start_lon, end_lat, end_lon, mode + ) + elif not start_off_network and end_off_network: + # Scenario C: on-network → off-network + return self._route_C_network_to_wilderness( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + elif start_off_network and not end_off_network: + # Scenario A: off-network → on-network + return self._route_A_wilderness_to_network( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + else: + # Scenario B: off-network → off-network + return self._route_B_wilderness_both( + start_lat, start_lon, end_lat, end_lon, mode, boundary_mode + ) + + def _route_D_network_only( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str + ) -> Dict: + """ + Scenario D: Both endpoints on-network. Pure Valhalla routing. + """ + t0 = time.time() + costing = MODE_TO_COSTING.get(mode, "pedestrian") + + valhalla_request = { + "locations": [ + {"lat": start_lat, "lon": start_lon}, + {"lat": end_lat, "lon": end_lon} + ], + "costing": costing, + "directions_options": {"units": "kilometers"} + } + + try: + resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) + + if resp.status_code != 200: + return { + "status": "error", + "message": f"Network routing failed: {resp.text[:200]}" + } + + valhalla_data = resp.json() + trip = valhalla_data.get("trip", {}) + legs = trip.get("legs", []) + + if not legs: + return {"status": "error", "message": "No route found"} + + leg = legs[0] + shape = leg.get("shape", "") + network_coords = self._decode_polyline(shape) + + maneuvers = [] + for m in leg.get("maneuvers", []): + maneuvers.append({ + "instruction": m.get("instruction", ""), + "type": m.get("type", 0), + "distance_km": m.get("length", 0), + "time_seconds": m.get("time", 0), + "street_names": m.get("street_names", []), + }) + + summary = trip.get("summary", {}) + distance_km = summary.get("length", 0) + duration_min = summary.get("time", 0) / 60 + + # Build response in same format as wilderness routes + network_feature = { + "type": "Feature", + "properties": { + "segment_type": "network", + "distance_km": distance_km, + "duration_minutes": duration_min, + "maneuvers": maneuvers, + "network_mode": mode, + }, + "geometry": {"type": "LineString", "coordinates": network_coords} + } + + combined_feature = { + "type": "Feature", + "properties": { + "segment_type": "combined", + "network_mode": mode, + }, + "geometry": {"type": "LineString", "coordinates": network_coords} + } + + geojson = {"type": "FeatureCollection", "features": [network_feature, combined_feature]} + + result = { + "status": "ok", + "route": geojson, + "summary": { + "total_distance_km": float(distance_km), + "total_effort_minutes": float(duration_min), + "wilderness_distance_km": 0.0, + "wilderness_effort_minutes": 0.0, + "network_distance_km": float(distance_km), + "network_duration_minutes": float(duration_min), + "on_trail_pct": 100.0, + "barrier_crossings": 0, + "network_mode": mode, + "scenario": "D", + "computation_time_s": time.time() - t0, + } + } + return result + + except Exception as e: + return {"status": "error", "message": f"Network routing failed: {e}"} + + def _route_A_wilderness_to_network( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario A: Off-network start → on-network end. + Wilderness pathfinding from start to entry point, then Valhalla to end. + """ + t0 = time.time() + + # Ensure entry point index exists + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + # Get valid highway classes for this mode + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + + # Find entry points near start, filtered by mode + MAX_ENTRY_POINTS = 10 + entry_points = self.entry_index.query_radius( + start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + + if not entry_points: + entry_points = self.entry_index.query_radius( + start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points: + if mode == "vehicle": + msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try a different mode." + elif mode in ("mtb", "atv"): + msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km. Try foot mode." + else: + msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of start." + return {"status": "error", "message": msg} + + entry_points = entry_points[:MAX_ENTRY_POINTS] + + # Run wilderness pathfinding + wilderness_result = self._pathfind_wilderness( + start_lat, start_lon, end_lat, end_lon, + entry_points, boundary_mode, "start" + ) + + if wilderness_result.get("status") == "error": + return wilderness_result + + # Extract results + wilderness_coords = wilderness_result["coords"] + wilderness_stats = wilderness_result["stats"] + best_entry = wilderness_result["entry_point"] + + entry_lat = best_entry["lat"] + entry_lon = best_entry["lon"] + + # Call Valhalla from entry point to destination + network_result = self._valhalla_route(entry_lat, entry_lon, end_lat, end_lon, mode) + + # Build response + return self._build_response( + wilderness_start=wilderness_coords, + wilderness_start_stats=wilderness_stats, + network_segment=network_result.get("segment"), + wilderness_end=None, + wilderness_end_stats=None, + mode=mode, + boundary_mode=boundary_mode, + entry_start=best_entry, + entry_end=None, + scenario="A", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _route_C_network_to_wilderness( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario C: On-network start → off-network end. + Valhalla from start to entry point, then wilderness pathfinding to end. + """ + t0 = time.time() + + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + + # Find entry points near END (destination) + MAX_ENTRY_POINTS = 10 + entry_points = self.entry_index.query_radius( + end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + + if not entry_points: + entry_points = self.entry_index.query_radius( + end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points: + if mode == "vehicle": + msg = f"No roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try a different mode." + elif mode in ("mtb", "atv"): + msg = f"No tracks or roads found within {EXPANDED_SEARCH_RADIUS_KM}km of destination. Try foot mode." + else: + msg = f"No trail entry points found within {EXPANDED_SEARCH_RADIUS_KM}km of destination." + return {"status": "error", "message": msg} + + entry_points = entry_points[:MAX_ENTRY_POINTS] + + # 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" + ) + + if wilderness_result.get("status") == "error": + return wilderness_result + + # The path is from end→entry, reverse it for display (entry→end) + wilderness_coords = list(reversed(wilderness_result["coords"])) + wilderness_stats = wilderness_result["stats"] + best_entry = wilderness_result["entry_point"] + + entry_lat = best_entry["lat"] + entry_lon = best_entry["lon"] + + # Call Valhalla from start to entry point + network_result = self._valhalla_route(start_lat, start_lon, entry_lat, entry_lon, mode) + + # Build response (network first, then wilderness) + return self._build_response( + wilderness_start=None, + wilderness_start_stats=None, + network_segment=network_result.get("segment"), + wilderness_end=wilderness_coords, + wilderness_end_stats=wilderness_stats, + mode=mode, + boundary_mode=boundary_mode, + entry_start=None, + entry_end=best_entry, + scenario="C", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _route_B_wilderness_both( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str, boundary_mode: str + ) -> Dict: + """ + Scenario B: Off-network start → off-network end. + Wilderness from start to entry_A, Valhalla entry_A to entry_B, wilderness from entry_B to end. + """ + t0 = time.time() + + if not self.entry_index.table_exists() or self.entry_index.get_entry_point_count() == 0: + return { + "status": "error", + "message": "Trail entry point index not built. Run build_entry_index() first." + } + + valid_highways = MODE_TO_VALID_HIGHWAYS.get(mode) + MAX_ENTRY_POINTS = 10 + + # Find entry points near START + entry_points_start = self.entry_index.query_radius( + start_lat, start_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_start: + entry_points_start = self.entry_index.query_radius( + start_lat, start_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_start: + return {"status": "error", "message": f"No entry points found near start within {EXPANDED_SEARCH_RADIUS_KM}km."} + entry_points_start = entry_points_start[:MAX_ENTRY_POINTS] + + # Find entry points near END + entry_points_end = self.entry_index.query_radius( + end_lat, end_lon, DEFAULT_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_end: + entry_points_end = self.entry_index.query_radius( + end_lat, end_lon, EXPANDED_SEARCH_RADIUS_KM, valid_highways + ) + if not entry_points_end: + return {"status": "error", "message": f"No entry points found near destination within {EXPANDED_SEARCH_RADIUS_KM}km."} + entry_points_end = entry_points_end[:MAX_ENTRY_POINTS] + + # 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" + ) + + if wilderness_start_result.get("status") == "error": + return wilderness_start_result + + wilderness_start_coords = wilderness_start_result["coords"] + wilderness_start_stats = wilderness_start_result["stats"] + entry_A = wilderness_start_result["entry_point"] + + # 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" + ) + + if wilderness_end_result.get("status") == "error": + return wilderness_end_result + + # Reverse the end wilderness path (it's end→entry, we want entry→end for display) + wilderness_end_coords = list(reversed(wilderness_end_result["coords"])) + wilderness_end_stats = wilderness_end_result["stats"] + entry_B = wilderness_end_result["entry_point"] + + # Phase 3: Valhalla from entry_A to entry_B + network_result = self._valhalla_route( + entry_A["lat"], entry_A["lon"], + entry_B["lat"], entry_B["lon"], + mode + ) + + # Build response + return self._build_response( + wilderness_start=wilderness_start_coords, + wilderness_start_stats=wilderness_start_stats, + network_segment=network_result.get("segment"), + wilderness_end=wilderness_end_coords, + wilderness_end_stats=wilderness_end_stats, + mode=mode, + boundary_mode=boundary_mode, + entry_start=entry_A, + entry_end=entry_B, + scenario="B", + t0=t0, + valhalla_error=network_result.get("error") + ) + + def _pathfind_wilderness( + self, + origin_lat: float, origin_lon: float, + dest_lat: float, dest_lon: float, + entry_points: List[Dict], + boundary_mode: str, + label: str + ) -> Dict: + """ + Run MCP wilderness pathfinding from origin toward entry points. + + Args: + origin_lat, origin_lon: Starting point for pathfinding + dest_lat, dest_lon: Ultimate destination (for bbox calculation) + entry_points: List of candidate entry points + boundary_mode: How to handle barriers + label: "start" or "end" for error messages + + Returns: + {"status": "ok", "coords": [...], "stats": {...}, "entry_point": {...}} + or {"status": "error", "message": "..."} + """ + # Build bbox - only include origin and entry points, NOT distant destination + # The destination is handled by Valhalla, wilderness only needs to reach entry points + MAX_BBOX_DEGREES = 2.0 + all_lats = [origin_lat] + [p["lat"] for p in entry_points] + all_lons = [origin_lon] + [p["lon"] for p in entry_points] + + padding = 0.05 + bbox = { + "south": min(all_lats) - padding, + "north": max(all_lats) + padding, + "west": min(all_lons) - padding, + "east": max(all_lons) + padding, + } + + # Clamp bbox size, centering on origin + lat_span = bbox["north"] - bbox["south"] + lon_span = bbox["east"] - bbox["west"] + if lat_span > MAX_BBOX_DEGREES or lon_span > MAX_BBOX_DEGREES: + half_span = MAX_BBOX_DEGREES / 2 + bbox = { + "south": origin_lat - half_span, + "north": origin_lat + half_span, + "west": origin_lon - half_span, + "east": origin_lon + half_span, + } + + # Initialize readers + self._init_readers() + + # Load elevation + try: + elevation, meta = self.dem_reader.get_elevation_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + ) + except Exception as e: + return {"status": "error", "message": f"Failed to load elevation for {label}: {e}"} + + # Check memory + mem = check_memory_usage() + if mem > MEMORY_LIMIT_GB: + return {"status": "error", "message": f"Memory limit exceeded: {mem:.1f}GB > {MEMORY_LIMIT_GB}GB"} + + # Load friction + friction_raw = self.friction_reader.get_friction_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape + ) + friction_mult = friction_to_multiplier(friction_raw) + + # Load barriers + barriers = self.barrier_reader.get_barrier_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape + ) + + # Load trails + trails = self.trail_reader.get_trails_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape + ) + + # Compute cost grid (ALWAYS foot mode for wilderness) + cost = compute_cost_grid( + elevation, + cell_size_m=meta["cell_size_m"], + friction=friction_mult, + friction_raw=friction_raw, + trails=trails, + barriers=barriers, + wilderness=None, + mvum=None, + boundary_mode=boundary_mode, + mode="foot", + ) + + # Free intermediate arrays + del friction_mult, friction_raw + gc.collect() + + # Convert origin to pixel coordinates + origin_row, origin_col = self.dem_reader.latlon_to_pixel(origin_lat, origin_lon, meta) + + rows, cols = elevation.shape + 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 + entry_pixels = [] + for ep in entry_points: + row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) + if 0 <= row < rows and 0 <= col < cols: + entry_pixels.append({"row": row, "col": col, "entry_point": ep}) + + 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)]) + + # Find nearest reachable entry point + best_entry = None + best_cost = np.inf + + 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): + 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"])) + + # Convert to coordinates and collect stats + coords = [] + elevations = [] + trail_values = [] + barrier_crossings = 0 + + for row, col in path_indices: + lat, lon = self.dem_reader.pixel_to_latlon(row, col, meta) + coords.append([lon, lat]) + elevations.append(elevation[row, col]) + trail_values.append(trails[row, col]) + if barriers[row, col] == 255: + barrier_crossings += 1 + + # Calculate distance + distance_m = 0 + for i in range(1, len(coords)): + lon1, lat1 = coords[i-1] + lon2, lat2 = coords[i] + distance_m += haversine_distance(lat1, lon1, lat2, lon2) + + # Elevation stats + elev_arr = np.array(elevations) + elev_diff = np.diff(elev_arr) + elev_gain = float(np.sum(elev_diff[elev_diff > 0])) + elev_loss = float(np.sum(np.abs(elev_diff[elev_diff < 0]))) + + # Trail stats + trail_arr = np.array(trail_values) + on_trail_cells = np.sum(trail_arr > 0) + total_cells = len(trail_arr) + 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 + gc.collect() + + return { + "status": "ok", + "coords": coords, + "stats": { + "distance_km": distance_m / 1000, + "effort_minutes": best_cost / 60, + "elevation_gain_m": elev_gain, + "elevation_loss_m": elev_loss, + "on_trail_pct": on_trail_pct, + "barrier_crossings": barrier_crossings, + "cell_count": total_cells, + }, + "entry_point": best_entry["entry_point"] + } + + def _valhalla_route( + self, + start_lat: float, start_lon: float, + end_lat: float, end_lon: float, + mode: str + ) -> Dict: + """ + Call Valhalla for network routing. + + Returns: + {"segment": {...}, "error": None} on success + {"segment": None, "error": "..."} on failure + """ + costing = MODE_TO_COSTING.get(mode, "pedestrian") + + valhalla_request = { + "locations": [ + {"lat": start_lat, "lon": start_lon}, + {"lat": end_lat, "lon": end_lon} + ], + "costing": costing, + "directions_options": {"units": "kilometers"} + } + + try: + resp = requests.post(f"{VALHALLA_URL}/route", json=valhalla_request, timeout=30) + + if resp.status_code == 200: + valhalla_data = resp.json() + trip = valhalla_data.get("trip", {}) + legs = trip.get("legs", []) + + if legs: + leg = legs[0] + shape = leg.get("shape", "") + coords = self._decode_polyline(shape) + + maneuvers = [] + for m in leg.get("maneuvers", []): + maneuvers.append({ + "instruction": m.get("instruction", ""), + "type": m.get("type", 0), + "distance_km": m.get("length", 0), + "time_seconds": m.get("time", 0), + "street_names": m.get("street_names", []), + }) + + summary = trip.get("summary", {}) + return { + "segment": { + "coordinates": coords, + "distance_km": summary.get("length", 0), + "duration_minutes": summary.get("time", 0) / 60, + "maneuvers": maneuvers, + }, + "error": None + } + + return {"segment": None, "error": f"Valhalla returned {resp.status_code}: {resp.text[:200]}"} + + except Exception as e: + return {"segment": None, "error": f"Valhalla request failed: {e}"} + + def _build_response( + self, + wilderness_start: Optional[List], + wilderness_start_stats: Optional[Dict], + network_segment: Optional[Dict], + wilderness_end: Optional[List], + wilderness_end_stats: Optional[Dict], + mode: str, + boundary_mode: str, + entry_start: Optional[Dict], + entry_end: Optional[Dict], + scenario: str, + t0: float, + valhalla_error: Optional[str] + ) -> Dict: + """Build the final GeoJSON response.""" + features = [] + + # Wilderness start segment + if wilderness_start and wilderness_start_stats: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "wilderness", + "segment_position": "start", + "effort_minutes": float(wilderness_start_stats["effort_minutes"]), + "distance_km": float(wilderness_start_stats["distance_km"]), + "elevation_gain_m": wilderness_start_stats["elevation_gain_m"], + "elevation_loss_m": wilderness_start_stats["elevation_loss_m"], + "boundary_mode": boundary_mode, + "on_trail_pct": wilderness_start_stats["on_trail_pct"], + "barrier_crossings": wilderness_start_stats["barrier_crossings"], + "wilderness_mode": "foot", + }, + "geometry": {"type": "LineString", "coordinates": wilderness_start} + }) + + # Network segment + if network_segment: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "network", + "distance_km": network_segment["distance_km"], + "duration_minutes": network_segment["duration_minutes"], + "maneuvers": network_segment["maneuvers"], + "network_mode": mode, + }, + "geometry": {"type": "LineString", "coordinates": network_segment["coordinates"]} + }) + + # Wilderness end segment + if wilderness_end and wilderness_end_stats: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "wilderness", + "segment_position": "end", + "effort_minutes": float(wilderness_end_stats["effort_minutes"]), + "distance_km": float(wilderness_end_stats["distance_km"]), + "elevation_gain_m": wilderness_end_stats["elevation_gain_m"], + "elevation_loss_m": wilderness_end_stats["elevation_loss_m"], + "boundary_mode": boundary_mode, + "on_trail_pct": wilderness_end_stats["on_trail_pct"], + "barrier_crossings": wilderness_end_stats["barrier_crossings"], + "wilderness_mode": "foot", + }, + "geometry": {"type": "LineString", "coordinates": wilderness_end} + }) + + # Combined path + combined_coords = [] + if wilderness_start: + combined_coords.extend(wilderness_start) + if network_segment: + # Skip first coord if we already have wilderness_start (avoid duplicate) + start_idx = 1 if wilderness_start else 0 + combined_coords.extend(network_segment["coordinates"][start_idx:]) + if wilderness_end: + # Skip first coord (avoid duplicate with network end) + start_idx = 1 if (wilderness_start or network_segment) else 0 + combined_coords.extend(wilderness_end[start_idx:]) + + if combined_coords: + features.append({ + "type": "Feature", + "properties": { + "segment_type": "combined", + "wilderness_mode": "foot", + "network_mode": mode, + "boundary_mode": boundary_mode, + "scenario": scenario, + }, + "geometry": {"type": "LineString", "coordinates": combined_coords} + }) + + geojson = {"type": "FeatureCollection", "features": features} + + # Calculate totals + total_distance_km = 0.0 + total_effort_minutes = 0.0 + wilderness_distance_km = 0.0 + wilderness_effort_minutes = 0.0 + network_distance_km = 0.0 + network_duration_minutes = 0.0 + barrier_crossings = 0 + on_trail_pct = 0.0 + + if wilderness_start_stats: + wilderness_distance_km += wilderness_start_stats["distance_km"] + wilderness_effort_minutes += wilderness_start_stats["effort_minutes"] + barrier_crossings += wilderness_start_stats["barrier_crossings"] + on_trail_pct = wilderness_start_stats["on_trail_pct"] + + if wilderness_end_stats: + wilderness_distance_km += wilderness_end_stats["distance_km"] + wilderness_effort_minutes += wilderness_end_stats["effort_minutes"] + barrier_crossings += wilderness_end_stats["barrier_crossings"] + # Average on-trail percentage if we have both + if wilderness_start_stats: + on_trail_pct = (on_trail_pct + wilderness_end_stats["on_trail_pct"]) / 2 + else: + on_trail_pct = wilderness_end_stats["on_trail_pct"] + + if network_segment: + network_distance_km = network_segment["distance_km"] + network_duration_minutes = network_segment["duration_minutes"] + + total_distance_km = wilderness_distance_km + network_distance_km + total_effort_minutes = wilderness_effort_minutes + network_duration_minutes + + summary = { + "total_distance_km": float(total_distance_km), + "total_effort_minutes": float(total_effort_minutes), + "wilderness_distance_km": float(wilderness_distance_km), + "wilderness_effort_minutes": float(wilderness_effort_minutes), + "network_distance_km": float(network_distance_km), + "network_duration_minutes": float(network_duration_minutes), + "on_trail_pct": float(on_trail_pct), + "barrier_crossings": barrier_crossings, + "boundary_mode": boundary_mode, + "wilderness_mode": "foot", + "network_mode": mode, + "scenario": scenario, + "computation_time_s": time.time() - t0, + } + + if entry_start: + summary["entry_point_start"] = { + "lat": entry_start["lat"], + "lon": entry_start["lon"], + "highway_class": entry_start["highway_class"], + "name": entry_start.get("name", ""), + } + + if entry_end: + summary["entry_point_end"] = { + "lat": entry_end["lat"], + "lon": entry_end["lon"], + "highway_class": entry_end["highway_class"], + "name": entry_end.get("name", ""), + } + + result = {"status": "ok", "route": geojson, "summary": summary} + + if valhalla_error: + result["warning"] = f"Network segment incomplete: {valhalla_error}" + + return result + + def _decode_polyline(self, encoded: str, precision: int = 6) -> List[List[float]]: + """Decode a polyline string into coordinates [lon, lat].""" + coords = [] + index = 0 + lat = 0 + lon = 0 + + while index < len(encoded): + shift = 0 + result = 0 + while True: + b = ord(encoded[index]) - 63 + index += 1 + result |= (b & 0x1f) << shift + shift += 5 + if b < 0x20: + break + dlat = ~(result >> 1) if result & 1 else result >> 1 + lat += dlat + + shift = 0 + result = 0 + while True: + b = ord(encoded[index]) - 63 + index += 1 + result |= (b & 0x1f) << shift + shift += 5 + if b < 0x20: + break + dlon = ~(result >> 1) if result & 1 else result >> 1 + lon += dlon + + coords.append([lon / (10 ** precision), lat / (10 ** precision)]) + + return coords + + def close(self): + """Close all readers.""" + if self.dem_reader: + self.dem_reader.close() + if self.friction_reader: + self.friction_reader.close() + if self.barrier_reader: + self.barrier_reader.close() + if self.wilderness_reader: + self.wilderness_reader.close() + if self.trail_reader: + self.trail_reader.close() + self.entry_index.close() + + +def build_entry_index(): + """Build the trail entry point index.""" + index = EntryPointIndex() + stats = index.build_index() + index.close() + return stats + + +if __name__ == "__main__": + import sys + + if len(sys.argv) > 1 and sys.argv[1] == "build": + print("Building trail entry point index...") + stats = build_entry_index() + print(f"\nDone. Total entry points: {stats['total']}") + + elif len(sys.argv) > 1 and sys.argv[1] == "test": + print("Testing router (all scenarios)...") + print("=" * 60) + + router = OffrouteRouter() + + # Test points + wilderness_start = (44.0543, -115.4237) # Off-network + wilderness_end = (45.2, -115.5) # Deep wilderness (Frank Church) + road_start = (43.6150, -116.2023) # Boise downtown (on-network) + road_end = (43.5867, -116.5625) # Nampa (on-network) + + tests = [ + ("A: wilderness→road", wilderness_start, (44.0814, -115.5021)), + ("B: wilderness→wilderness", wilderness_start, wilderness_end), + ("C: road→wilderness", road_start, wilderness_start), + ("D: road→road", road_start, road_end), + ] + + for label, (slat, slon), (elat, elon) in tests: + print(f"\n{label}") + print("-" * 40) + + result = router.route( + start_lat=slat, start_lon=slon, + end_lat=elat, end_lon=elon, + mode="foot", boundary_mode="pragmatic" + ) + + if result["status"] == "ok": + s = result["summary"] + print(f" Scenario: {s.get('scenario', '?')}") + print(f" Total: {s['total_distance_km']:.2f} km, {s['total_effort_minutes']:.1f} min") + print(f" Wilderness: {s['wilderness_distance_km']:.2f} km") + print(f" Network: {s['network_distance_km']:.2f} km") + if s.get('entry_point_start'): + ep = s['entry_point_start'] + print(f" Entry (start): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") + if s.get('entry_point_end'): + ep = s['entry_point_end'] + print(f" Entry (end): {ep['highway_class']} at {ep['lat']:.4f}, {ep['lon']:.4f}") + else: + print(f" ERROR: {result['message']}") + + router.close() + + else: + print("Usage:") + print(" python router.py build # Build entry point index") + print(" python router.py test # Test all scenarios") From d8f84ab55aa8f6e910ed6f2a52aae2032a70c91b Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 9 May 2026 03:34:37 +0000 Subject: [PATCH 15/16] offroute: revert off-network threshold to 10m Co-Authored-By: Claude Opus 4.5 --- lib/offroute/router.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index 4b988ab..bdb7894 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -61,7 +61,7 @@ EXPANDED_SEARCH_RADIUS_KM = 100 MEMORY_LIMIT_GB = 12 # Off-network detection threshold (meters) -OFF_NETWORK_THRESHOLD_M = 50 +OFF_NETWORK_THRESHOLD_M = 10 # Mode to Valhalla costing mapping MODE_TO_COSTING = { From a04c10ad556ba5e5d915c402bd003a6e5d6db609 Mon Sep 17 00:00:00 2001 From: Matt Date: Sat, 9 May 2026 05:05:00 +0000 Subject: [PATCH 16/16] offroute: wilderness maneuvers with bearing, elevation, grade MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Segment breaks on: bearing change >30°, grade category change, distance >0.5mi - Grade categories: flat (0-2°), gentle (2-5°), moderate (5-10°), steep (10-15°), very steep (15°+) - Distance formatting: feet with commas <1mi, miles with decimal ≥1mi - Instruction format: Head {cardinal}, gaining/descending X ft ({grade} uphill/downhill) — {dist} Co-Authored-By: Claude --- lib/offroute/router.py | 209 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 209 insertions(+) diff --git a/lib/offroute/router.py b/lib/offroute/router.py index bdb7894..bd3d379 100644 --- a/lib/offroute/router.py +++ b/lib/offroute/router.py @@ -728,6 +728,7 @@ class OffrouteRouter: # Extract results wilderness_coords = wilderness_result["coords"] wilderness_stats = wilderness_result["stats"] + wilderness_elevations = wilderness_result.get("elevations", []) best_entry = wilderness_result["entry_point"] entry_lat = best_entry["lat"] @@ -740,9 +741,11 @@ class OffrouteRouter: return self._build_response( wilderness_start=wilderness_coords, wilderness_start_stats=wilderness_stats, + wilderness_start_elevations=wilderness_elevations, network_segment=network_result.get("segment"), wilderness_end=None, wilderness_end_stats=None, + wilderness_end_elevations=None, mode=mode, boundary_mode=boundary_mode, entry_start=best_entry, @@ -805,6 +808,7 @@ class OffrouteRouter: # The path is from end→entry, reverse it for display (entry→end) wilderness_coords = list(reversed(wilderness_result["coords"])) wilderness_stats = wilderness_result["stats"] + wilderness_elevations = list(reversed(wilderness_result.get("elevations", []))) best_entry = wilderness_result["entry_point"] entry_lat = best_entry["lat"] @@ -817,9 +821,11 @@ class OffrouteRouter: return self._build_response( wilderness_start=None, wilderness_start_stats=None, + wilderness_start_elevations=None, network_segment=network_result.get("segment"), wilderness_end=wilderness_coords, wilderness_end_stats=wilderness_stats, + wilderness_end_elevations=wilderness_elevations, mode=mode, boundary_mode=boundary_mode, entry_start=None, @@ -885,6 +891,7 @@ class OffrouteRouter: wilderness_start_coords = wilderness_start_result["coords"] wilderness_start_stats = wilderness_start_result["stats"] + wilderness_start_elevations = wilderness_start_result.get("elevations", []) entry_A = wilderness_start_result["entry_point"] # Phase 2: Wilderness pathfinding from END (run after freeing phase 1 memory) @@ -899,6 +906,7 @@ class OffrouteRouter: # Reverse the end wilderness path (it's end→entry, we want entry→end for display) wilderness_end_coords = list(reversed(wilderness_end_result["coords"])) wilderness_end_stats = wilderness_end_result["stats"] + wilderness_end_elevations = list(reversed(wilderness_end_result.get("elevations", []))) entry_B = wilderness_end_result["entry_point"] # Phase 3: Valhalla from entry_A to entry_B @@ -912,9 +920,11 @@ class OffrouteRouter: return self._build_response( wilderness_start=wilderness_start_coords, wilderness_start_stats=wilderness_start_stats, + wilderness_start_elevations=wilderness_start_elevations, network_segment=network_result.get("segment"), wilderness_end=wilderness_end_coords, wilderness_end_stats=wilderness_end_stats, + wilderness_end_elevations=wilderness_end_elevations, mode=mode, boundary_mode=boundary_mode, entry_start=entry_A, @@ -1109,6 +1119,7 @@ class OffrouteRouter: return { "status": "ok", "coords": coords, + "elevations": elevations, # Raw elevation values for maneuver generation "stats": { "distance_km": distance_m / 1000, "effort_minutes": best_cost / 60, @@ -1184,13 +1195,199 @@ class OffrouteRouter: except Exception as e: return {"segment": None, "error": f"Valhalla request failed: {e}"} + def _generate_wilderness_maneuvers( + self, + coords: List[List[float]], + elevations: List[float], + position: str = "start" + ) -> List[Dict]: + """ + Generate turn-by-turn maneuvers for a wilderness segment. + + Segment breaks occur when: + - Bearing changes more than 30° from segment start + - Grade category changes (flat→steep etc) + - Distance exceeds 0.5 miles without a break + + Args: + coords: [[lon, lat], ...] coordinate list + elevations: Elevation values (meters) for each coord + position: "start" or "end" for labeling + + Returns: + List of maneuver dicts with instruction, distance, elevation, grade, bearing + """ + if not coords or len(coords) < 2: + return [] + + # Constants + COMPASS = ["N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", + "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"] + MAX_SEGMENT_M = 804.672 # 0.5 miles in meters + BEARING_THRESHOLD = 30 # degrees + M_TO_FT = 3.28084 + M_TO_MI = 0.000621371 + + def get_bearing(lat1, lon1, lat2, lon2): + """Calculate bearing between two points (degrees 0-360).""" + dlon = math.radians(lon2 - lon1) + lat1_r, lat2_r = math.radians(lat1), math.radians(lat2) + x = math.sin(dlon) * math.cos(lat2_r) + y = math.cos(lat1_r) * math.sin(lat2_r) - math.sin(lat1_r) * math.cos(lat2_r) * math.cos(dlon) + return (math.degrees(math.atan2(x, y)) + 360) % 360 + + def bearing_to_cardinal(bearing): + """Convert bearing to 16-point compass direction.""" + return COMPASS[round(bearing / 22.5) % 16] + + def get_grade_category(grade_deg): + """Categorize grade angle: flat (0-2°), gentle (2-5°), moderate (5-10°), steep (10-15°), very steep (15°+).""" + grade_abs = abs(grade_deg) + if grade_abs < 2: + return "flat" + elif grade_abs < 5: + return "gentle" + elif grade_abs < 10: + return "moderate" + elif grade_abs < 15: + return "steep" + else: + return "very steep" + + def format_distance(meters): + """Format distance: feet with commas if under 1 mile, miles with one decimal if over.""" + miles = meters * M_TO_MI + if miles < 1.0: + feet = round(meters * M_TO_FT) + return f"{feet:,} ft" + else: + return f"{miles:.1f} mi" + + def build_instruction(cardinal, gain_ft, loss_ft, grade_cat, distance_m): + """Build instruction string per spec.""" + dist_str = format_distance(distance_m) + if grade_cat == "flat": + return f"Head {cardinal} on level ground — {dist_str}" + elif gain_ft > loss_ft: + return f"Head {cardinal}, gaining {gain_ft:,} ft ({grade_cat} uphill) — {dist_str}" + else: + return f"Head {cardinal}, descending {loss_ft:,} ft ({grade_cat} downhill) — {dist_str}" + + maneuvers = [] + i = 0 + + while i < len(coords) - 1: + seg_start_idx = i + seg_start_lon, seg_start_lat = coords[i] + seg_start_elev = elevations[i] if i < len(elevations) else 0 + + # Initial bearing for this segment + next_lon, next_lat = coords[i + 1] + seg_bearing = get_bearing(seg_start_lat, seg_start_lon, next_lat, next_lon) + + # Accumulate elevation changes within segment + seg_distance_m = 0 + seg_elev_gain = 0 + seg_elev_loss = 0 + prev_elev = seg_start_elev + + # Calculate initial grade category + step_dist = haversine_distance(seg_start_lat, seg_start_lon, next_lat, next_lon) + step_elev_change = (elevations[i + 1] if i + 1 < len(elevations) else seg_start_elev) - seg_start_elev + initial_grade = math.degrees(math.atan(step_elev_change / step_dist)) if step_dist > 0 else 0 + seg_grade_cat = get_grade_category(initial_grade) + + j = i + while j < len(coords) - 1: + lon1, lat1 = coords[j] + lon2, lat2 = coords[j + 1] + elev1 = elevations[j] if j < len(elevations) else prev_elev + elev2 = elevations[j + 1] if j + 1 < len(elevations) else elev1 + + step_dist = haversine_distance(lat1, lon1, lat2, lon2) + step_bearing = get_bearing(lat1, lon1, lat2, lon2) + step_elev_change = elev2 - elev1 + step_grade = math.degrees(math.atan(step_elev_change / step_dist)) if step_dist > 0 else 0 + step_grade_cat = get_grade_category(step_grade) + + # Check break conditions + bearing_diff = abs(step_bearing - seg_bearing) + if bearing_diff > 180: + bearing_diff = 360 - bearing_diff + + # Break if: bearing changed >30°, grade category changed, or distance >0.5mi + if seg_distance_m > 0: # Don't break on first step + if bearing_diff > BEARING_THRESHOLD: + break + if step_grade_cat != seg_grade_cat: + break + if seg_distance_m >= MAX_SEGMENT_M: + break + + # Accumulate + seg_distance_m += step_dist + if step_elev_change > 0: + seg_elev_gain += step_elev_change + else: + seg_elev_loss += abs(step_elev_change) + prev_elev = elev2 + j += 1 + + # Compute segment stats + seg_end_idx = j + gain_ft = round(seg_elev_gain * M_TO_FT) + loss_ft = round(seg_elev_loss * M_TO_FT) + + # Net elevation change for grade calculation + net_elev_change = seg_elev_gain - seg_elev_loss + grade_deg = math.degrees(math.atan(net_elev_change / seg_distance_m)) if seg_distance_m > 0 else 0 + grade_cat = get_grade_category(grade_deg) + + cardinal = bearing_to_cardinal(seg_bearing) + instruction = build_instruction(cardinal, gain_ft, loss_ft, grade_cat, seg_distance_m) + + maneuvers.append({ + "instruction": instruction, + "type": "wilderness", + "distance_m": round(seg_distance_m, 1), + "elevation_gain_ft": gain_ft, + "elevation_loss_ft": loss_ft, + "grade_degrees": round(grade_deg, 1), + "grade_category": grade_cat, + "bearing": round(seg_bearing, 1), + "cardinal": cardinal, + }) + + i = seg_end_idx + + # Add arrival maneuver + arrival_text = "Arrive at trail/road" if position == "start" else "Arrive at destination" + last_bearing = maneuvers[-1]["bearing"] if maneuvers else 0 + last_cardinal = maneuvers[-1]["cardinal"] if maneuvers else "N" + + maneuvers.append({ + "instruction": arrival_text, + "type": "arrival", + "distance_m": 0, + "elevation_gain_ft": 0, + "elevation_loss_ft": 0, + "grade_degrees": 0, + "grade_category": "flat", + "bearing": last_bearing, + "cardinal": last_cardinal, + }) + + return maneuvers + def _build_response( self, wilderness_start: Optional[List], wilderness_start_stats: Optional[Dict], + wilderness_start_elevations: Optional[List], network_segment: Optional[Dict], wilderness_end: Optional[List], wilderness_end_stats: Optional[Dict], + wilderness_end_elevations: Optional[List], mode: str, boundary_mode: str, entry_start: Optional[Dict], @@ -1204,6 +1401,11 @@ class OffrouteRouter: # Wilderness start segment if wilderness_start and wilderness_start_stats: + wild_start_maneuvers = [] + if wilderness_start_elevations: + wild_start_maneuvers = self._generate_wilderness_maneuvers( + wilderness_start, wilderness_start_elevations, position="start" + ) features.append({ "type": "Feature", "properties": { @@ -1217,6 +1419,7 @@ class OffrouteRouter: "on_trail_pct": wilderness_start_stats["on_trail_pct"], "barrier_crossings": wilderness_start_stats["barrier_crossings"], "wilderness_mode": "foot", + "maneuvers": wild_start_maneuvers, }, "geometry": {"type": "LineString", "coordinates": wilderness_start} }) @@ -1237,6 +1440,11 @@ class OffrouteRouter: # Wilderness end segment if wilderness_end and wilderness_end_stats: + wild_end_maneuvers = [] + if wilderness_end_elevations: + wild_end_maneuvers = self._generate_wilderness_maneuvers( + wilderness_end, wilderness_end_elevations, position="end" + ) features.append({ "type": "Feature", "properties": { @@ -1250,6 +1458,7 @@ class OffrouteRouter: "on_trail_pct": wilderness_end_stats["on_trail_pct"], "barrier_crossings": wilderness_end_stats["barrier_crossings"], "wilderness_mode": "foot", + "maneuvers": wild_end_maneuvers, }, "geometry": {"type": "LineString", "coordinates": wilderness_end} })