feat(simulation): add Worker AI polygon runtime and terrain navigation

simulation/ai-polygon/navigation/Dockerfile

@@ -0,0 +1,34 @@
+FROM ros:humble-ros-base-jammy
+
+LABEL com.nodedc.product="mission-core" \
+      com.nodedc.stack="ai-polygon" \
+      com.nodedc.role="terrain-navigation" \
+      com.nodedc.managed-by="ai-polygon-worker" \
+      org.opencontainers.image.source="https://github.com/HongbiaoZ/autonomous_exploration_development_environment" \
+      org.opencontainers.image.revision="158e67b31b644ed1e8b06eb1d7f70e183cc62591"
+
+SHELL ["/bin/bash", "-o", "pipefail", "-c"]
+RUN apt-get update && apt-get install -y --no-install-recommends \
+      curl ca-certificates build-essential python3-numpy python3-yaml \
+      python3-colcon-common-extensions ros-humble-pcl-ros \
+      ros-humble-pcl-conversions ros-humble-tf2-geometry-msgs \
+      ros-humble-sensor-msgs-py ros-humble-rclpy \
+    && rm -rf /var/lib/apt/lists/*
+
+# Upstream's terrain analysis and collision-free motion primitives are unchanged.
+# Keep the exact sources and their declared BSD package licenses in the image.
+WORKDIR /opt/cmu
+RUN curl -fL --retry 3 \
+      https://codeload.github.com/HongbiaoZ/autonomous_exploration_development_environment/tar.gz/158e67b31b644ed1e8b06eb1d7f70e183cc62591 \
+      -o /tmp/upstream.tgz \
+    && mkdir upstream src \
+    && tar -xzf /tmp/upstream.tgz -C upstream --strip-components=1 \
+    && cp -a upstream/src/local_planner upstream/src/terrain_analysis src/ \
+    && source /opt/ros/humble/setup.bash \
+    && MAKEFLAGS=-j2 colcon build --base-paths src --parallel-workers 1 \
+      --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF \
+    && rm /tmp/upstream.tgz
+
+ENV ROS_LOCALHOST_ONLY=1 ROS_DOMAIN_ID=83
+WORKDIR /adapter
+ENTRYPOINT ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash && source /opt/cmu/install/setup.bash && exec python3 /adapter/navigation/server.py"]

simulation/ai-polygon/navigation/Dockerfile.connectivity

@@ -0,0 +1,8 @@
+FROM ndc-ai-polygon-cmu:rectangle-v3
+COPY terrain_connectivity.cpp /opt/missioncore/terrain_connectivity.cpp
+RUN g++ -O3 -std=c++17 -ffp-contract=off -fno-fast-math -fPIC -shared \
+    /opt/missioncore/terrain_connectivity.cpp -o /opt/missioncore/libterrain_connectivity.so
+LABEL com.nodedc.product="mission-core" \
+      com.nodedc.stack="ai-polygon" \
+      com.nodedc.role="terrain-navigation" \
+      com.nodedc.managed-by="ai-polygon-worker"

simulation/ai-polygon/navigation/Dockerfile.footprint

@@ -0,0 +1,16 @@
+# Reuse the already-built pinned upstream image. The caller supplies its exact
+# sha256 through a local immutable build tag; no model images are replaced.
+ARG CMU_BASE
+FROM ${CMU_BASE}
+LABEL com.nodedc.product="mission-core" \
+      com.nodedc.stack="ai-polygon" \
+      com.nodedc.role="terrain-navigation" \
+      com.nodedc.managed-by="ai-polygon-worker" \
+      com.nodedc.navigation-adapter="rectangular-primitive-filter-v3"
+COPY missioncore_footprint.hpp /opt/cmu/src/local_planner/src/missioncore_footprint.hpp
+COPY patch_footprint.py /opt/cmu/patch_footprint.py
+WORKDIR /opt/cmu
+RUN python3 patch_footprint.py && source /opt/ros/humble/setup.bash \
+    && MAKEFLAGS=-j2 colcon build --base-paths src --packages-select local_planner \
+        --parallel-workers 1 --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF
+WORKDIR /adapter

simulation/ai-polygon/navigation/Prepare-Connectivity.ps1

@@ -0,0 +1,17 @@
+param([string]$Root = 'D:\NDC_MISSIONCORE\runtime\simulation')
+$ErrorActionPreference = 'Continue'
+$base = 'sha256:d9b9c842bf8f0bf5208d8568d35f41b69d6e4a9ed74f31dedbcd455f1f7f263f'
+$actual = docker image inspect 'ndc-ai-polygon-cmu:rectangle-v3' --format '{{.Id}}'
+if ($LASTEXITCODE -ne 0 -or $actual.Trim() -ne $base) { throw 'Prepare the pinned rectangular CMU image first' }
+$endpoint = docker context inspect --format '{{.Endpoints.docker.Host}}'
+if ($LASTEXITCODE -ne 0) { throw 'Docker endpoint unavailable' }
+$config = Join-Path $Root 'state\connectivity-public-build'
+New-Item -ItemType Directory -Force $config | Out-Null
+[IO.File]::WriteAllText((Join-Path $config 'config.json'), '{"auths":{"https://index.docker.io/v1/":{}}}')
+$env:DOCKER_CONFIG = $config
+$env:BUILDX_CONFIG = Join-Path $config 'buildx'
+docker --config $config --host $endpoint.Trim() build --progress plain -t ndc-ai-polygon-cmu:connectivity-v1 -f (Join-Path $PSScriptRoot 'Dockerfile.connectivity') $PSScriptRoot
+if ($LASTEXITCODE -ne 0) { throw 'Terrain connectivity build failed' }
+$image = docker --config $config --host $endpoint.Trim() image inspect ndc-ai-polygon-cmu:connectivity-v1 --format '{{.Id}}'
+if ($LASTEXITCODE -ne 0) { throw 'Built image unavailable' }
+@{base_image=$base; image=$image.Trim(); source_sha256=(Get-FileHash (Join-Path $PSScriptRoot 'terrain_connectivity.cpp') -Algorithm SHA256).Hash.ToLower(); installed_at=[DateTime]::UtcNow.ToString('o')} | ConvertTo-Json

simulation/ai-polygon/navigation/Prepare-Navigation.ps1

@@ -0,0 +1,34 @@
+param([string]$Root = 'D:\NDC_MISSIONCORE\runtime\simulation')
+# Windows PowerShell 5 treats Docker's normal stderr progress as ErrorRecords.
+# Native failures are checked by exit code after each command below.
+$ErrorActionPreference = 'Continue'
+$ProgressPreference = 'SilentlyContinue'
+$tag = 'ndc/mission-core-ai-module-cmu-navigation:158e67b3-v1'
+$receipt = Join-Path $Root 'assets\cmu-navigation-v1.json'
+$existing = docker image inspect $tag --format '{{.Id}}' 2>$null
+if ($LASTEXITCODE -ne 0) {
+    # Public upstream images need no credentials. Windows SSH cannot access the
+    # interactive user's credential helper; isolate only this build's config.
+    $endpoint = docker context inspect --format '{{.Endpoints.docker.Host}}'
+    if ($LASTEXITCODE -ne 0) { throw 'Docker endpoint unavailable' }
+    $buildConfig = Join-Path $PSScriptRoot 'public-build-config'
+    New-Item -ItemType Directory -Force $buildConfig | Out-Null
+    # An explicit anonymous registry prevents CLI auto-discovery of wincred.
+    [IO.File]::WriteAllText((Join-Path $buildConfig 'config.json'), '{"auths":{"https://index.docker.io/v1/":{}}}')
+    $env:DOCKER_CONFIG = $buildConfig
+    $env:BUILDX_CONFIG = Join-Path $buildConfig 'buildx'
+    docker --config $buildConfig --host $endpoint.Trim() build --progress plain -t $tag -f (Join-Path $PSScriptRoot 'Dockerfile') $PSScriptRoot
+    if ($LASTEXITCODE -ne 0) { throw 'CMU navigation image build failed' }
+    $existing = docker image inspect $tag --format '{{.Id}}'
+    if ($LASTEXITCODE -ne 0) { throw 'Built navigation image is unavailable' }
+}
+$value = @{
+    schema_version = 'missioncore.ai-polygon-navigation-install/v1'
+    upstream = 'HongbiaoZ/autonomous_exploration_development_environment'
+    upstream_commit = '158e67b31b644ed1e8b06eb1d7f70e183cc62591'
+    image = $existing.Trim()
+    tag = $tag
+    installed_at = [DateTime]::UtcNow.ToString('o')
+}
+[IO.File]::WriteAllText($receipt, ($value | ConvertTo-Json -Depth 5), [Text.UTF8Encoding]::new($false))
+$value | ConvertTo-Json -Compress

simulation/ai-polygon/navigation/Prepare-RectangleNavigation.ps1

@@ -0,0 +1,24 @@
+param([string]$Root = 'D:\NDC_MISSIONCORE\runtime\simulation')
+$ErrorActionPreference = 'Continue'
+$base = 'sha256:7693c28fac83df9e9e6ac6d89ffec81470f660cb8b30fc78fa88be228329dc0b'
+$baseTag = 'ndc-ai-polygon-cmu:upstream-158e67b'
+$tag = 'ndc-ai-polygon-cmu:rectangle-v3'
+$actual = docker image inspect $base --format '{{.Id}}'
+if ($LASTEXITCODE -ne 0 -or $actual.Trim() -ne $base) { throw 'Prepare the pinned upstream CMU image first' }
+docker tag $base $baseTag
+if ($LASTEXITCODE -ne 0) { throw 'Unable to identify the local CMU build base' }
+docker build --build-arg "CMU_BASE=$baseTag" -f (Join-Path $PSScriptRoot 'Dockerfile.footprint') -t $tag $PSScriptRoot
+if ($LASTEXITCODE -ne 0) { throw 'Rectangular candidate filter build failed' }
+$image = docker image inspect $tag --format '{{.Id}}'
+if ($LASTEXITCODE -ne 0) { throw 'Built navigation image unavailable' }
+$value = @{
+    schema_version = 'missioncore.ai-polygon-navigation-install/v1'
+    upstream_commit = '158e67b31b644ed1e8b06eb1d7f70e183cc62591'
+    base_image = $base
+    image = $image.Trim()
+    adapter = 'rectangular-primitive-filter-v3'
+    adapter_sha256 = (Get-FileHash (Join-Path $PSScriptRoot 'missioncore_footprint.hpp') -Algorithm SHA256).Hash.ToLower()
+    installed_at = [DateTime]::UtcNow.ToString('o')
+}
+[IO.File]::WriteAllText((Join-Path $Root 'assets\cmu-navigation-rectangle-v3.json'), ($value | ConvertTo-Json), [Text.UTF8Encoding]::new($false))
+$value | ConvertTo-Json -Compress

simulation/ai-polygon/navigation/Prepare-Terrain.ps1

@@ -0,0 +1,74 @@
+param(
+    [Parameter(Mandatory=$true)][string]$WorldFile,
+    [string]$Root = 'D:\NDC_MISSIONCORE\runtime\simulation',
+    [string]$Python = 'D:\NDC_MISSIONCORE\runtime\simulation\isaac-sim-6.1.0\kit\python\python.exe'
+)
+$ErrorActionPreference = 'Stop'
+[Threading.Thread]::CurrentThread.CurrentCulture = [Globalization.CultureInfo]::InvariantCulture
+$world = Get-Content -Raw $WorldFile | ConvertFrom-Json
+if ($world.sha256 -notmatch '^[a-f0-9]{64}$') { throw 'Invalid source hash' }
+$source = Join-Path $Root ('state\worlds\' + $world.sha256 + '.ply')
+if ((Get-FileHash $source -Algorithm SHA256).Hash.ToLower() -ne $world.sha256) { throw 'World source changed' }
+$tool = Join-Path $Root 'tools\splat-transform-3.4.0\node_modules\@playcanvas\splat-transform'
+$package = Get-Content -Raw (Join-Path $tool 'package.json') | ConvertFrom-Json
+if ($package.version -ne '3.4.0') { throw 'SplatTransform version changed' }
+$settings = $world.settings
+$json = ($settings | ConvertTo-Json -Compress) + (Get-FileHash $PSCommandPath -Algorithm SHA256).Hash
+$sha = [Security.Cryptography.SHA256]::Create()
+$digest = [BitConverter]::ToString($sha.ComputeHash([Text.Encoding]::UTF8.GetBytes($json))).Replace('-','').ToLower()
+$out = Join-Path $Root ('assets\terrain-v1\' + $world.sha256 + '-' + $digest.Substring(0,16))
+New-Item -ItemType Directory -Force $out | Out-Null
+$mesh = Join-Path $out 'terrain.collision.glb'
+$migrator = Join-Path $PSScriptRoot 'migrate_terrain_coordinates.py'
+if ((Get-FileHash $migrator -Algorithm SHA256).Hash.ToLower() -ne 'ff08de8b97b9f2b0040b379840c402f5c7c327e3bc1915548d99b6372ff5ca37') { throw 'Coordinate migrator identity changed' }
+if (-not (Test-Path $mesh)) {
+    # Exact rigid correction of the admitted historical cache avoids rerunning
+    # a faulty third-party BVH build. Never mutates or reclassifies that mesh.
+    $ErrorActionPreference = 'Continue'
+    & $Python $migrator --root $Root --world $WorldFile --output $mesh
+    if ($LASTEXITCODE -notin @(0,2)) { throw 'Collision coordinate migration failed' }
+    $ErrorActionPreference = 'Stop'
+}
+if (-not (Test-Path $mesh)) {
+    # SplatTransform 3.4 assigns PLY an implicit Rz(180) before CLI actions.
+    # Cancel it FIRST, then apply the same XYZ rotation as the USD visual.
+    # World Z-up -> glTF Y-up is Rx(-90): (x,y,z) -> (x,z,-y).
+    # Runtime's (x,y,z) -> (x,-z,y) is exactly its inverse. Do not use Rx(+90).
+    $x = [double]$settings.spawn_xy[0]; $y = [double]$settings.spawn_xy[1]; $z = [double]$settings.ground_z
+    $box = @(($x-15),($z-4),(-$y-15),($x+15),($z+8),(-$y+15)) -join ','
+    $args = @('--max-old-space-size=12288', (Join-Path $tool 'bin\cli.mjs'), $source,
+        '--filter-nan', '--rotate', '0,0,180',
+        '--rotate', ($settings.rotation_degrees -join ','),
+        '--scale', [string]$settings.meters_per_unit,
+        '--rotate', '-90,0,0', '--filter-box', $box,
+        '--voxel-size', '0.06', '--voxel-opacity', '0.25',
+        '--voxel-floor-fill', '0.3', '--collision-mesh', 'smooth',
+        (Join-Path $out 'terrain.voxel.json'))
+    $ErrorActionPreference = 'Continue'
+    & node @args
+    if ($LASTEXITCODE -ne 0) { throw 'SplatTransform collision generation failed' }
+    $ErrorActionPreference = 'Stop'
+}
+if (-not (Test-Path $mesh)) { throw 'Collision mesh was not produced' }
+$correctionPath = Join-Path $out 'coordinate-correction.json'
+$correction = if (Test-Path $correctionPath) { Get-Content -Raw $correctionPath | ConvertFrom-Json } else { $null }
+if ($correction) {
+    if ((Get-FileHash $mesh -Algorithm SHA256).Hash.ToLower() -ne $correction.collider_sha256) { throw 'Corrected collision changed' }
+    $settings = $correction.settings
+}
+$manifest = @{
+    schema_version = 'missioncore.ai-polygon-terrain/v1'
+    source_sha256 = $world.sha256
+    settings = $settings
+    generator = '@playcanvas/splat-transform@3.4.0'
+    generator_sha256 = (Get-FileHash $PSCommandPath -Algorithm SHA256).Hash.ToLower()
+    voxel_size_m = 0.06
+    collider = $mesh
+    collider_sha256 = (Get-FileHash $mesh -Algorithm SHA256).Hash.ToLower()
+    coordinate_system = 'gltf-y-up-from-metric-world'
+    coordinate_revision = 'ply-rz180-cancelled-usd-xyz-gltf-rx-minus90-v2'
+    coordinate_correction = $correction
+    qualification = 'reconstructed-proxy-requires-contact-validation'
+}
+[IO.File]::WriteAllText((Join-Path $out 'terrain.json'), ($manifest | ConvertTo-Json -Depth 6), [Text.UTF8Encoding]::new($false))
+$manifest | ConvertTo-Json -Depth 6 -Compress

simulation/ai-polygon/navigation/fastdds.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
+  <transport_descriptors>
+    <transport_descriptor>
+      <transport_id>worker_loopback</transport_id>
+      <type>UDPv4</type>
+      <sendBufferSize>1048576</sendBufferSize>
+      <receiveBufferSize>1048576</receiveBufferSize>
+      <interfaceWhiteList><address>127.0.0.1</address></interfaceWhiteList>
+    </transport_descriptor>
+  </transport_descriptors>
+  <participant profile_name="worker_navigation" is_default_profile="true">
+    <rtps>
+      <userTransports><transport_id>worker_loopback</transport_id></userTransports>
+      <useBuiltinTransports>false</useBuiltinTransports>
+    </rtps>
+  </participant>
+</profiles>

simulation/ai-polygon/navigation/footprint.py

@@ -0,0 +1,107 @@
+"""Metre-square swept-body check on CMU's observed terrain and chosen path.
+
+CMU's circular path table proposes paths. This final adapter check preserves
+the actual square chassis, including the initial turn, without widening it to
+its circumscribed circle for straight travel. No scene geometry enters here.
+"""
+
+import math
+
+import numpy as np
+
+MAX_STEP_M = 0.10
+FRAME_DEADLINE_SECONDS = 0.8
+
+
+def _obstacles(terrain, pose):
+    terrain = np.asarray(terrain, dtype=float)
+    obstacles = terrain[terrain[:, 3] > MAX_STEP_M + 1e-4, :2] - np.asarray(pose[:2])
+    x, y, z, w = pose[3:]
+    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
+    return obstacles @ np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
+
+
+def _clearances(obstacles):
+    return np.maximum(np.abs(obstacles[:, 0]) - 0.5, np.abs(obstacles[:, 1]) - 0.5)
+
+
+def _clear(obstacles, position, angle):
+    delta = obstacles - position
+    c, s = math.cos(angle), math.sin(angle)
+    along = delta[:, 0] * c + delta[:, 1] * s
+    across = -delta[:, 0] * s + delta[:, 1] * c
+    initial = _clearances(obstacles)
+    if np.any(initial <= 0):
+        return False  # Never excuse an overlap of the actual metre-square body.
+    clearance = np.maximum(np.abs(along) - 0.5, np.abs(across) - 0.5)
+    # An observed point may already be inside the 5 cm reserve behind the body.
+    # Permit only motion that never decreases that initial clearance. This
+    # cannot authorize moving toward it, reversing into it or corner penetration.
+    return bool(np.all(clearance + 1e-6 >= np.minimum(initial, 0.05)))
+
+
+def command_footprint_clear(speed, yaw_rate, terrain, pose):
+    """Collision monitor over deadman latency plus a conservative braking arc.
+
+    CMU replans the route continuously. A later blocked corner must not prevent
+    safe progress on its prefix; this checks the command that can actually be
+    applied before the source-frame deadline, plus braking and 0.5 s reserve.
+    """
+    obstacles = _obstacles(terrain, pose)
+    horizon = FRAME_DEADLINE_SECONDS + 0.5 + abs(speed) / 0.4 + abs(yaw_rate) / 1.6
+    for t in np.arange(0, horizon + 0.025, 0.025):
+        angle = yaw_rate * t
+        position = (
+            np.array([speed * math.sin(angle) / yaw_rate, speed * (1 - math.cos(angle)) / yaw_rate])
+            if abs(yaw_rate) > 1e-6
+            else np.array([speed * t, 0])
+        )
+        if not _clear(obstacles, position, angle):
+            return False
+    return True
+
+
+def regulate_command(speed, yaw_rate, terrain, pose):
+    """Reduce speed along CMU's same arc when its full-speed stop is unsafe.
+
+    Scaling both components preserves curvature. The shortened stopping envelope
+    is a prefix of the original arc, so search for its largest admitted scale.
+    Never choose another turn/direction, ignore a hazard or creep arbitrarily.
+    """
+    if command_footprint_clear(speed, yaw_rate, terrain, pose):
+        return speed, yaw_rate, 1.0
+    low, high = 0.2, 1.0
+    if not command_footprint_clear(speed * low, yaw_rate * low, terrain, pose):
+        return 0.0, 0.0, 0.0
+    for _ in range(7):
+        middle = (low + high) / 2
+        if command_footprint_clear(speed * middle, yaw_rate * middle, terrain, pose):
+            low = middle
+        else:
+            high = middle
+    return speed * low, yaw_rate * low, low
+
+
+def swept_footprint_clear(path, terrain, pose):
+    path = np.asarray(path, dtype=float)[:, :2]
+    terrain = np.asarray(terrain, dtype=float)
+    if len(path) < 2 or terrain.ndim != 2 or terrain.shape[1] != 4:
+        return False
+    obstacles = _obstacles(terrain, pose)
+
+    previous_angle = 0.0
+    for start, end in zip(path[:-1], path[1:], strict=True):
+        delta = end - start
+        length = np.linalg.norm(delta)
+        if length < 1e-6:
+            continue
+        angle = math.atan2(delta[1], delta[0])
+        turn = math.atan2(math.sin(angle - previous_angle), math.cos(angle - previous_angle))
+        for fraction in np.linspace(0, 1, max(2, math.ceil(abs(turn) / 0.035) + 1)):
+            if not _clear(obstacles, start, previous_angle + fraction * turn):
+                return False
+        for fraction in np.linspace(0, 1, max(2, math.ceil(length / 0.025) + 1)):
+            if not _clear(obstacles, start + fraction * delta, angle):
+                return False
+        previous_angle = angle
+    return True

simulation/ai-polygon/navigation/migrate_terrain_coordinates.py

@@ -0,0 +1,120 @@
+"""Correct only the known v1 cache basis; original collision assets are immutable.
+
+For the admitted [-90,0,180] scene rotation, the old and correct pipelines
+have identical vertical axes and differ by a horizontal 180-degree rotation.
+Rotating the finished mesh is exact: no filtering, smoothing or new geometry.
+"""
+
+import argparse
+import hashlib
+import json
+import struct
+from pathlib import Path
+
+LEGACY_GENERATOR = "d7e142184c1de43cde71ebc7cd311e667c18c7c7a17725c14e123127740697a2"
+
+
+def rotate_glb(data):
+    magic, version, size = struct.unpack_from("<III", data)
+    if (magic, version, size) != (0x46546C67, 2, len(data)):
+        raise ValueError("Invalid collision GLB")
+    chunks = {}
+    offset = 12
+    while offset < size:
+        length, kind = struct.unpack_from("<II", data, offset)
+        chunks[kind] = data[offset + 8 : offset + 8 + length]
+        offset += 8 + length
+    document = json.loads(chunks[0x4E4F534A])
+    binary = bytearray(chunks[0x004E4942])
+    if any(
+        any(k in node for k in ("matrix", "rotation", "translation", "scale"))
+        for node in document.get("nodes", [])
+    ):
+        raise ValueError("Collision transforms must be baked")
+    transformed = set()
+    for mesh in document["meshes"]:
+        for primitive in mesh["primitives"]:
+            if set(primitive["attributes"]) != {"POSITION"}:
+                raise ValueError("Only the known position-only collision export is admitted")
+            index = primitive["attributes"]["POSITION"]
+            if index in transformed:
+                continue
+            transformed.add(index)
+            accessor = document["accessors"][index]
+            view = document["bufferViews"][accessor["bufferView"]]
+            if (
+                accessor["componentType"] != 5126
+                or accessor["type"] != "VEC3"
+                or "sparse" in accessor
+                or view.get("buffer", 0) != 0
+            ):
+                raise ValueError("Unsupported collision position buffer")
+            start = view.get("byteOffset", 0) + accessor.get("byteOffset", 0)
+            stride = view.get("byteStride", 12)
+            for i in range(accessor["count"]):
+                at = start + i * stride
+                x, y, z = struct.unpack_from("<fff", binary, at)
+                struct.pack_into("<fff", binary, at, -x, y, -z)
+            low, high = accessor["min"], accessor["max"]
+            accessor["min"] = [-high[0], low[1], -high[2]]
+            accessor["max"] = [-low[0], high[1], -low[2]]
+    encoded = json.dumps(document, separators=(",", ":")).encode()
+    encoded += b" " * ((-len(encoded)) % 4)
+    result = struct.pack("<III", 0x46546C67, 2, 12 + 8 + len(encoded) + 8 + len(binary))
+    return (
+        result
+        + struct.pack("<II", len(encoded), 0x4E4F534A)
+        + encoded
+        + struct.pack("<II", len(binary), 0x004E4942)
+        + binary
+    )
+
+
+def migrate(root, world, output):
+    settings = world["settings"]
+    if settings["rotation_degrees"] != [-90, 0, 180]:
+        return False
+    for path in sorted((root / "assets/terrain-v1").glob(world["sha256"] + "-*/terrain.json")):
+        manifest = json.loads(path.read_text(encoding="utf-8-sig"))
+        old = manifest["settings"]
+        if (
+            manifest.get("generator_sha256") != LEGACY_GENERATOR
+            or old["rotation_degrees"] != [-90, 0, 180]
+            or old["meters_per_unit"] != settings["meters_per_unit"]
+        ):
+            continue
+        centre = [-v for v in old["spawn_xy"]]
+        if (
+            any(abs(centre[i] - settings["spawn_xy"][i]) > 12 for i in (0, 1))
+            or abs(old["ground_z"] - settings["ground_z"]) > 2
+        ):
+            continue
+        original = Path(manifest["collider"]).read_bytes()
+        if hashlib.sha256(original).hexdigest() != manifest["collider_sha256"]:
+            raise ValueError("Legacy collision identity changed")
+        corrected = rotate_glb(original)
+        temporary = output.with_suffix(".part.glb")
+        temporary.write_bytes(corrected)
+        temporary.replace(output)
+        report = {
+            "source_manifest": str(path),
+            "source_collider_sha256": manifest["collider_sha256"],
+            "collider_sha256": hashlib.sha256(corrected).hexdigest(),
+            "transform": "gltf-rotate-y-180",
+            "settings": {**old, "spawn_xy": centre},
+        }
+        output.with_name("coordinate-correction.json").write_text(
+            json.dumps(report, indent=2), encoding="utf-8"
+        )
+        return True
+    return False
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--root", type=Path, required=True)
+    parser.add_argument("--world", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    args = parser.parse_args()
+    if not migrate(args.root, json.loads(args.world.read_text(encoding="utf-8-sig")), args.output):
+        raise SystemExit(2)

simulation/ai-polygon/navigation/missioncore_footprint.hpp

@@ -0,0 +1,74 @@
+// Mission Core adapter for CMU's existing motion-primitive selector.
+// Reject a candidate before selection when its initial turn or first metre
+// sweeps the real rectangular body through observed terrain. Upstream's
+// angular wedge alone misses obstacles beside a rear corner.
+#pragma once
+#include <algorithm>
+#include <cmath>
+#include <vector>
+#include <utility>
+
+template <class TerrainCloud, class PathCloud>
+bool missioncoreFootprintClear(const TerrainCloud& terrain, const PathCloud& path,
+                              double rotation, double scale, double range,
+                              double length, double width, double threshold,
+                              bool twoWayDrive) {
+  const double margin = 0.05;
+  const double halfLength = length / 2.0 - margin, halfWidth = width / 2.0 - margin;
+  const double cap = std::min(range, 1.2);
+  std::vector<std::pair<double, double>> obstacles;
+  for (const auto& p : terrain.points) {
+    if (p.intensity > threshold && std::hypot(p.x, p.y) < cap + std::hypot(halfLength + margin, halfWidth + margin))
+      obstacles.emplace_back(p.x, p.y);
+  }
+  auto clear = [&](double x, double y, double yaw) {
+    const double c = std::cos(yaw), s = std::sin(yaw);
+    for (const auto& p : obstacles) {
+      const double dx = p.first - x, dy = p.second - y;
+      const double initial = std::max(std::abs(p.first) - halfLength,
+                                      std::abs(p.second) - halfWidth);
+      if (initial <= 0) return false;
+      const double clearance = std::max(std::abs(dx * c + dy * s) - halfLength,
+                                        std::abs(-dx * s + dy * c) - halfWidth);
+      if (clearance + 1e-6 < std::min(margin, initial)) return false;
+    }
+    return true;
+  };
+  auto turnClear = [&](double x, double y, double from, double to) {
+    const double delta = std::atan2(std::sin(to - from), std::cos(to - from));
+    const int steps = std::max(1, int(std::ceil(std::abs(delta) / 0.025)));
+    for (int i = 0; i <= steps; ++i)
+      if (!clear(x, y, from + delta * i / steps)) return false;
+    return true;
+  };
+  if (path.points.size() < 2 || !clear(0, 0, 0)) return false;
+  // Match pathFollower's 0.7 m look-ahead. Its initial command points here,
+  // which need not equal the selected primitive's initial tangent.
+  double lookYaw = rotation;
+  for (const auto& p : path.points) {
+    lookYaw = rotation + std::atan2(p.y, p.x);
+    if (std::hypot(p.x, p.y) * scale >= std::min(0.7, cap)) break;
+  }
+  // A reversing body follows the rear-facing primitive without first turning
+  // 180 degrees. Match pathFollower's twoWayDrive direction selection.
+  const bool reverse = twoWayDrive && std::cos(lookYaw) < 0;
+  if (reverse) lookYaw += M_PI;
+  if (!turnClear(0, 0, 0, lookYaw)) return false;
+  const double c = std::cos(rotation), s = std::sin(rotation);
+  double lastX = 0, lastY = 0, lastYaw = lookYaw;
+  for (const auto& p : path.points) {
+    if (std::hypot(p.x, p.y) * scale > cap) break;
+    const double x = scale * (c * p.x - s * p.y);
+    const double y = scale * (s * p.x + c * p.y);
+    const double distance = std::hypot(x - lastX, y - lastY);
+    if (distance < 1e-6) continue;
+    const double yaw = std::atan2(y - lastY, x - lastX) + (reverse ? M_PI : 0);
+    if (!turnClear(lastX, lastY, lastYaw, yaw)) return false;
+    const int steps = std::max(1, int(std::ceil(distance / 0.025)));
+    for (int i = 1; i <= steps; ++i)
+      if (!clear(lastX + (x - lastX) * i / steps,
+                 lastY + (y - lastY) * i / steps, yaw)) return false;
+    lastX = x; lastY = y; lastYaw = yaw;
+  }
+  return true;
+}

simulation/ai-polygon/navigation/patch_footprint.py

@@ -0,0 +1,14 @@
+"""Apply one explicit adapter patch to the pinned CMU selector; fail on drift."""
+from pathlib import Path
+
+path = Path("/opt/cmu/src/local_planner/src/localPlanner.cpp")
+source = path.read_text()
+needle = "            maxScore = clearPathPerGroupScore[i];\n            selectedGroupID = i;"
+replacement = """\
+            if (!missioncoreFootprintClear(*plannerCloudCrop, *startPaths[i % groupNum],
+                rotAng, pathScale, std::min(pathRange, relativeGoalDis),
+                vehicleLength, vehicleWidth, obstacleHeightThre, twoWayDrive)) continue;
+            maxScore = clearPathPerGroupScore[i];
+            selectedGroupID = i;"""
+assert source.count(needle) == 1, "CMU selector source changed"
+path.write_text('#include "missioncore_footprint.hpp"\n' + source.replace(needle, replacement))

simulation/ai-polygon/navigation/qualify.py

@@ -0,0 +1,294 @@
+"""Bounded synthetic acceptance of the installed CMU image on Worker only.
+
+This tests navigation independently of perception and physics. It does not
+claim Gaussian-world or physical-rover acceptance. Owns one temporary container.
+"""
+
+import argparse
+import http.client
+import json
+import math
+import subprocess
+import time
+from datetime import UTC, datetime
+from pathlib import Path
+from uuid import uuid4
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--adapter", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    args = parser.parse_args()
+    profile = json.loads((args.adapter / "models.worker-006.json").read_text())
+    name = "ndc-mission-core-ai-module-navigation-check-" + uuid4().hex[:8]
+    identity = subprocess.check_output(
+        [
+            "docker",
+            "run",
+            "-d",
+            "--name",
+            name,
+            "--cpus",
+            "3",
+            "--memory",
+            "2g",
+            "--label",
+            "com.nodedc.stack=ai-polygon-qualification",
+            "--label",
+            "com.nodedc.product=mission-core",
+            "--label",
+            "com.nodedc.role=qualification",
+            "--label",
+            "com.nodedc.managed-by=ai-polygon-qualification",
+            "-p",
+            "127.0.0.1:18193:8010",
+            "--mount",
+            f"type=bind,source={args.adapter},target=/adapter,readonly",
+            profile["navigation"]["image"],
+        ],
+        text=True,
+    ).strip()
+    connection = http.client.HTTPConnection("127.0.0.1", 18193, timeout=3)
+
+    def request(path, body=None):
+        connection.request(
+            "GET" if body is None else "POST", path, body=None if body is None else json.dumps(body)
+        )
+        response = connection.getresponse()
+        value = json.loads(response.read())
+        if response.status != 200:
+            raise RuntimeError(str(value))
+        return value
+
+    def ready():
+        deadline = time.monotonic() + 15
+        while time.monotonic() < deadline:
+            try:
+                request("/ready")
+                return
+            except (OSError, RuntimeError, http.client.HTTPException):
+                time.sleep(0.2)
+        raise TimeoutError("CMU qualification container did not become ready")
+
+    report = {
+        "image": profile["navigation"]["image"],
+        "cases": [],
+        "utc": datetime.now(UTC).isoformat(),
+        "monotonic": time.monotonic(),
+    }
+    try:
+        ready()
+        floor = [[x / 10, y / 10, 0.0] for x in range(-20, 41) for y in range(-20, 21)]
+        wall = [[1.5, y / 10, z / 10] for y in range(-20, 21) for z in range(1, 16)]
+        enclosure = [
+            [side * 0.9, y / 10, z / 10]
+            for side in (-1, 1)
+            for y in range(-9, 10)
+            for z in range(1, 16)
+        ]
+        enclosure += [
+            [x / 10, side * 0.9, z / 10]
+            for side in (-1, 1)
+            for x in range(-9, 10)
+            for z in range(1, 16)
+        ]
+
+        def corridor(half_width):
+            return floor + [
+                [x / 10, side * half_width, z / 10]
+                for x in range(-15, 51)
+                for side in (-1, 1)
+                for z in range(1, 16)
+            ]
+
+        for name, points in (
+            ("clear-flat-ground", floor),
+            ("rear-reserve-forward-escape", floor + [[-0.53, 0, z / 10] for z in range(1, 10)]),
+            ("front-reserve-stop", floor + [[0.53, 0, z / 10] for z in range(1, 10)]),
+            ("body-overlap-stop", floor + [[-0.49, 0, z / 10] for z in range(1, 10)]),
+            ("reverse-clear-ground", floor),
+            (
+                "reverse-blocked-by-rear-wall",
+                floor + [[-0.7, y / 10, z / 10] for y in range(-10, 11) for z in range(1, 12)],
+            ),
+            ("turn-away-from-wall", floor + wall),
+            (
+                "close-wall-stop",
+                floor + [[0.65, y / 10, z / 10] for y in range(-20, 21) for z in range(1, 16)],
+            ),
+            ("closed-enclosure-stop", floor + enclosure),
+            ("metre-rover-in-1.3m-corridor", corridor(0.65)),
+            ("metre-rover-rejects-0.9m-corridor", corridor(0.45)),
+            (
+                "rear-corner-selects-clear-primitive",
+                floor + [[-0.62, -0.39, z / 10] for z in range(1, 15)],
+            ),
+        ):
+            request("/reset", {})
+            ready()
+            samples = []
+            for _ in range(20):
+                samples.append(
+                    request(
+                        "/plan",
+                        {
+                            "points": points,
+                            "pose": [0, 0, 0.27, 0, 0, 0, 1],
+                            "goal": [-0.65, 0, 0]
+                            if name.startswith("reverse-")
+                            else ([2, -2, 0] if name.startswith("rear-corner") else [3, 0, 0]),
+                            "allow_reverse": name.startswith("reverse-"),
+                            "max_speed_mps": 0.15,
+                        },
+                    )
+                )
+                time.sleep(0.2)
+            if name in (
+                "clear-flat-ground",
+                "metre-rover-in-1.3m-corridor",
+                "rear-reserve-forward-escape",
+            ):
+                passed = any(v["speed_mps"] > 0.05 and v["status"] == "path" for v in samples)
+            elif name == "reverse-clear-ground":
+                passed = any(v["speed_mps"] < -0.05 and v["status"] == "path" for v in samples[-5:])
+            elif name == "turn-away-from-wall":
+                valid = [v for v in samples[-5:] if v["status"] == "path"]
+                passed = len(valid) >= 3 and all(
+                    (v["speed_mps"] > 0.02 or abs(v["yaw_rate_rps"]) > 0.1)
+                    and max(p[0] for p in v["path"]) < 0.75
+                    for v in valid
+                )
+            elif name.startswith("rear-corner"):
+                passed = any(v["speed_mps"] > 0.05 for v in samples[-5:]) and all(
+                    v.get("diagnostic", {}).get("failure") != "footprint" for v in samples[-5:]
+                )
+            else:
+                passed = all(v["speed_mps"] == 0 and v["status"] == "blocked" for v in samples[-5:])
+            report["cases"].append({"name": name, "passed": passed, "samples": samples})
+        # A vanished return is occlusion, not proof of free space. Observe a
+        # near wall, then only distant ground for longer than the decay timer.
+        request("/reset", {})
+        ready()
+        close_wall = [[0.7, y / 10, z / 10] for y in range(-10, 11) for z in range(1, 10)]
+        samples = []
+        for i in range(30):
+            samples.append(
+                request(
+                    "/plan",
+                    {
+                        "points": floor + close_wall if i < 5 else [p for p in floor if p[0] > 1.2],
+                        "pose": [0, 0, 0.37, 0, 0, 0, 1],
+                        "goal": [3, 0, 0],
+                        "max_speed_mps": 0.15,
+                    },
+                )
+            )
+            time.sleep(0.2)
+        report["cases"].append(
+            {
+                "name": "occluded-near-obstacle-retained",
+                "samples": samples,
+                "passed": all(
+                    x["speed_mps"] == 0 and x["status"] == "blocked" for x in samples[-5:]
+                ),
+            }
+        )
+        # Navigation must admit the same continuous grades as the measured
+        # physical profile, while retaining a discontinuous 15 cm ledge.
+        for angle, quantized in [(a, False) for a in (5, 10, 15, 20, 25)] + [
+            (10, True),
+            (20, True),
+        ]:
+            request("/reset", {})
+            ready()
+            radians = math.radians(angle)
+            slope = math.tan(radians)
+            samples = []
+            for _ in range(12):
+                samples.append(
+                    request(
+                        "/plan",
+                        {
+                            "points": [
+                                [x, y, round(x * slope / 0.06) * 0.06 if quantized else x * slope]
+                                for x, y, _ in floor
+                            ],
+                            "pose": [
+                                0,
+                                0,
+                                0.37,
+                                0,
+                                -math.sin(radians / 2),
+                                0,
+                                math.cos(radians / 2),
+                            ],
+                            "goal": [3, 0, 3 * slope],
+                            "max_speed_mps": 0.15,
+                        },
+                    )
+                )
+                time.sleep(0.2)
+            report["cases"].append(
+                {
+                    "name": f"supported-{'voxel-' if quantized else ''}ramp-{angle}-degrees",
+                    "samples": samples,
+                    "passed": any(s["speed_mps"] > 0.05 for s in samples[-5:]),
+                }
+            )
+        for height in (0.12, 0.15, -0.4):
+            request("/reset", {})
+            ready()
+            points = [[x, y, height if x >= 0.7 else 0] for x, y, _ in floor]
+            points += [
+                [x / 10, side * 0.65, z / 10]
+                for x in range(-15, 41)
+                for side in (-1, 1)
+                for z in range(1, 16)
+            ]
+            samples = []
+            for _ in range(12):
+                samples.append(
+                    request(
+                        "/plan",
+                        {
+                            "points": points,
+                            "pose": [0, 0, 0.37, 0, 0, 0, 1],
+                            "goal": [3, 0, 0],
+                            "max_speed_mps": 0.15,
+                        },
+                    )
+                )
+                time.sleep(0.2)
+            report["cases"].append(
+                {
+                    "name": f"discontinuous-height-{height:+.2f}m-stop",
+                    "samples": samples,
+                    "passed": all(
+                        s["speed_mps"] == 0 and s["status"] == "blocked" for s in samples[-5:]
+                    ),
+                }
+            )
+        report["passed"] = all(case["passed"] for case in report["cases"])
+    finally:
+        report["container_log_tail"] = subprocess.run(
+            ["docker", "logs", "--tail", "30", identity], capture_output=True, text=True
+        ).stderr
+        args.output.parent.mkdir(parents=True, exist_ok=True)
+        args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
+        connection.close()
+        subprocess.run(["docker", "rm", "-f", identity], check=True, capture_output=True)
+    print(
+        json.dumps(
+            {
+                "passed": report["passed"],
+                "cases": [
+                    {"name": row["name"], "passed": row["passed"]} for row in report["cases"]
+                ],
+            }
+        )
+    )
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/qualify_connectivity.py

@@ -0,0 +1,31 @@
+"""Worker-only numeric equivalence at hazard boundaries and disconnected patches."""
+
+import argparse
+import json
+from pathlib import Path
+
+import numpy as np
+import terrain_costs as costs
+
+parser = argparse.ArgumentParser()
+parser.add_argument("--output", type=Path, required=True)
+args = parser.parse_args()
+assert costs._NATIVE is not None, "Compiled implementation must be installed"
+engine = costs._NATIVE
+rng = np.random.default_rng(230923)
+cases = []
+for dtype in (np.float32, np.float64):
+    for _ in range(200):
+        points = rng.uniform(-0.4, 0.4, (rng.integers(8, 180), 3)).astype(dtype)
+        points[:, 2] *= 0.3
+        cases.append(points)
+    for distance in (0.119999, 0.12, 0.120001):
+        for jump in (0.100099, 0.1001, 0.100101):
+            cases.append(np.array([[0, 0, 0], [distance, 0, jump]], dtype=dtype))
+for points in cases:
+    costs._NATIVE = None
+    expected = costs.connected_grade(points)
+    costs._NATIVE = engine
+    assert costs.connected_grade(points) == expected
+args.output.write_text(json.dumps({"passed": True, "cases": len(cases)}))
+print(json.dumps({"passed": True, "cases": len(cases)}))

simulation/ai-polygon/navigation/qualify_ipc.py

@@ -0,0 +1,50 @@
+"""Bounded Windows snapshot read/replace acceptance; no simulator required."""
+
+import json
+import sys
+import tempfile
+import threading
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from local_state import StateChannel
+
+
+def main():
+    errors = []
+    done = threading.Event()
+    with tempfile.TemporaryDirectory(prefix="ndc-polygon-ipc-") as folder:
+        directory = Path(folder)
+        writer = StateChannel(directory)
+        writer.write("snapshot", {"sequence": -1})
+
+        def reader():
+            channel = StateChannel(directory)
+            try:
+                while not done.is_set():
+                    row = channel.read("snapshot")
+                    if row is None or not -1 <= row["sequence"] < 1000:
+                        raise RuntimeError("Incomplete snapshot read")
+            except Exception as exc:
+                errors.append(type(exc).__name__ + ": " + str(exc))
+            finally:
+                channel.close()
+
+        thread = threading.Thread(target=reader)
+        thread.start()
+        try:
+            for sequence in range(1000):
+                writer.write("snapshot", {"sequence": sequence, "telemetry": [sequence] * 100})
+        finally:
+            done.set()
+            thread.join()
+        if errors:
+            raise RuntimeError(str(errors))
+        if writer.read("snapshot")["sequence"] != 999:
+            raise RuntimeError("Final snapshot was not retained")
+        writer.close()
+    print(json.dumps({"passed": True, "concurrent_replacements": 1000}))
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/qualify_perception.py

@@ -0,0 +1,57 @@
+"""Offline numeric check of the pinned GOOSE decoder on retained Worker RGB.
+
+Runs in the existing pinned image, without changing its runner or checkpoint.
+This diagnoses the adapter; it is not semantic accuracy or navigation acceptance.
+"""
+
+import argparse
+import csv
+import hashlib
+import importlib.util
+import json
+import time
+from pathlib import Path
+
+import numpy as np
+import torch
+from PIL import Image
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--image", type=Path, required=True)
+    parser.add_argument("--output", type=Path, required=True)
+    args = parser.parse_args()
+    spec = importlib.util.spec_from_file_location("reference", "/assets/ddrnet-goose-runner.py")
+    module = importlib.util.module_from_spec(spec)
+    spec.loader.exec_module(module)
+    model, _, _ = module.load_model("ddrnet", Path("/assets/ddrnet-checkpoint.pth"))
+    tensor, _ = module.preprocess(Image.open(args.image))
+    tensor = tensor.cuda()
+    with torch.inference_mode():
+        logits = module.logits_from_output(model(tensor)).float()
+        legacy = torch.sigmoid(logits).argmax(1)
+        direct = logits.argmax(1)
+        saturated = (torch.sigmoid(logits) == 1).sum(1)
+    names = {}
+    with open("/assets/ddrnet-goose-mapping.csv") as stream:
+        names = {int(r["label_key"]): r["class_name"] for r in csv.DictReader(stream)}
+    args.output.mkdir(exist_ok=True, parents=True)
+    report = {
+        "source_sha256": hashlib.sha256(args.image.read_bytes()).hexdigest(),
+        "monotonic_ns": time.monotonic_ns(),
+        "logit_range": [float(logits.min()), float(logits.max())],
+        "changed_pixels": int((legacy != direct).sum()),
+        "saturated_tie_pixels": int((saturated > 1).sum()),
+    }
+    for name, mask in (("reference", legacy), ("direct", direct)):
+        mask = mask[0].cpu().numpy().astype(np.uint8)
+        Image.fromarray(mask).save(args.output / (name + ".png"))
+        ids, counts = np.unique(mask, return_counts=True)
+        report[name] = {names[int(i)]: int(counts[index]) for index, i in enumerate(ids)}
+    (args.output / "report.json").write_text(json.dumps(report, indent=2), encoding="utf-8")
+    print(json.dumps(report))
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/qualify_placement.py

@@ -0,0 +1,105 @@
+"""Offline authoring check for a stable, metre-wide start on a scan proxy.
+
+Uses world geometry only to prepare a scene. No candidate map enters navigation.
+An operator/engineer still verifies the chosen start against the visual trail.
+"""
+
+import argparse
+import json
+import math
+import sys
+from pathlib import Path
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from spawn_clearance import obstructing_triangles
+from terrain import load_glb
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--terrain", type=Path, required=True)
+    args = parser.parse_args()
+    manifest = json.loads((args.terrain / "terrain.json").read_text(encoding="utf-8-sig"))
+    settings = manifest["settings"]
+    points, indices = load_glb(args.terrain / "terrain.collision.glb")
+    triangles = points[indices]
+    low, high = triangles.min(axis=1), triangles.max(axis=1)
+    center = np.array(settings["spawn_xy"])
+    selected = (
+        (low[:, :2] <= center + 2.5).all(axis=1)
+        & (high[:, :2] >= center - 2.5).all(axis=1)
+        & (low[:, 2] < settings["ground_z"] + 1.5)
+        & (high[:, 2] > settings["ground_z"] - 0.8)
+    )
+    triangles, low, high = triangles[selected], low[selected], high[selected]
+
+    def heights(x, y):
+        hits = triangles[
+            (low[:, 0] <= x) & (high[:, 0] >= x) & (low[:, 1] <= y) & (high[:, 1] >= y)
+        ]
+        if not len(hits):
+            return np.empty(0)
+        a, b, c = hits[:, 0], hits[:, 1], hits[:, 2]
+        den = (b[:, 1] - c[:, 1]) * (a[:, 0] - c[:, 0]) + (c[:, 0] - b[:, 0]) * (a[:, 1] - c[:, 1])
+        valid = np.abs(den) > 1e-8
+        a, b, c, den = a[valid], b[valid], c[valid], den[valid]
+        u = ((b[:, 1] - c[:, 1]) * (x - c[:, 0]) + (c[:, 0] - b[:, 0]) * (y - c[:, 1])) / den
+        v = ((c[:, 1] - a[:, 1]) * (x - c[:, 0]) + (a[:, 0] - c[:, 0]) * (y - c[:, 1])) / den
+        inside = (u >= -1e-6) & (v >= -1e-6) & (u + v <= 1 + 1e-6)
+        return (u * a[:, 2] + v * b[:, 2] + (1 - u - v) * c[:, 2])[inside]
+
+    angle = math.radians(settings["heading_degrees"])
+    rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
+    footprint = np.array([[x, y] for x in (-0.5, 0, 0.5) for y in (-0.5, 0, 0.5)]) @ rotation.T
+    candidates = []
+    for dx in np.arange(-2, 2.01, 0.2):
+        for dy in np.arange(-2, 2.01, 0.2):
+            position = center + [dx, dy]
+            support = []
+            for x, y in footprint + position:
+                z = heights(x, y)
+                near = z[np.abs(z - settings["ground_z"]) < 0.8]
+                if not len(near):
+                    break
+                ground = near.max()
+                if np.any((z > ground + 0.12) & (z < ground + 1)):
+                    break
+                support.append(float(ground))
+            if len(support) != 9:
+                continue
+            design = np.column_stack((footprint, np.ones(9)))
+            plane = np.linalg.lstsq(design, np.asarray(support), rcond=None)[0]
+            residual = float(np.max(np.abs(design @ plane - support)))
+            slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
+            if residual > 0.08 or slope > 20:
+                continue
+            if obstructing_triangles(
+                points, indices, position, settings["heading_degrees"], plane
+            ):
+                continue
+            candidates.append(
+                {
+                    "xy": position.tolist(),
+                    "ground_z": float(np.median(support)),
+                    "height_span": max(support) - min(support),
+                    "offset_m": math.hypot(dx, dy),
+                    "residual_m": residual,
+                    "slope_degrees": slope,
+                }
+            )
+    candidates.sort(key=lambda row: row["offset_m"] + 2 * row["height_span"])
+    print(
+        json.dumps(
+            {
+                "world_sha256": manifest["source_sha256"],
+                "candidate_count": len(candidates),
+                "candidates": candidates[:12],
+            }
+        )
+    )
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/replay.py

@@ -0,0 +1,121 @@
+"""Replay retained sensor evidence against the shipped CMU adapter on Worker.
+
+Owns exactly one bounded CPU container. Never starts rendering or model GPU jobs.
+"""
+
+import argparse
+import hashlib
+import http.client
+import json
+import subprocess
+import time
+from collections import Counter
+from datetime import UTC, datetime
+from pathlib import Path
+from uuid import uuid4
+
+import numpy as np
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument("--adapter", type=Path, required=True)
+    p.add_argument("--run", type=Path, required=True)
+    p.add_argument("--output", type=Path, required=True)
+    p.add_argument("--limit", type=int, default=180)
+    p.add_argument("--terrain", action="store_true")
+    args = p.parse_args()
+    profile = json.loads((args.adapter / "models.worker-006.json").read_text())
+    identity = subprocess.check_output(
+        [
+            "docker",
+            "run",
+            "-d",
+            "--name",
+            "ndc-ai-polygon-navigation-replay-" + uuid4().hex[:8],
+            "--cpus",
+            "3",
+            "--memory",
+            "2g",
+            "--label",
+            "com.nodedc.product=mission-core",
+            "--label",
+            "com.nodedc.stack=ai-polygon-qualification",
+            "--label",
+            "com.nodedc.role=qualification",
+            "--label",
+            "com.nodedc.managed-by=ai-polygon-qualification",
+            "-p",
+            "127.0.0.1:18193:8010",
+            "--mount",
+            f"type=bind,source={args.adapter},target=/adapter,readonly",
+            profile["navigation"]["image"],
+        ],
+        text=True,
+    ).strip()
+    connection = http.client.HTTPConnection("127.0.0.1", 18193, timeout=3)
+    source = args.run / "camera/decisions.jsonl"
+    rows = [json.loads(line) for line in source.read_text().splitlines()]
+    report = dict(
+        utc=datetime.now(UTC).isoformat(),
+        monotonic=time.monotonic(),
+        input_sha256=hashlib.sha256(source.read_bytes()).hexdigest(),
+        samples=[],
+    )
+    try:
+        deadline = time.monotonic() + 20
+        while True:
+            try:
+                connection.request("GET", "/ready")
+                response = connection.getresponse()
+                response.read()
+                if response.status == 200:
+                    break
+            except (OSError, http.client.HTTPException):
+                connection.close()
+            if time.monotonic() > deadline:
+                raise TimeoutError("Replay navigation unavailable")
+            time.sleep(0.2)
+        for row in rows[: args.limit]:
+            if row["goal"] is None:
+                continue
+            observation = np.load(args.run / "camera" / f"{row['frame_id']:08d}.range.npz")
+            payload = dict(
+                points=observation["points"].tolist(),
+                pose=observation["pose"].tolist(),
+                goal=row["goal"],
+                max_speed_mps=0.15,
+                allow_reverse=row.get("mission", {}).get("state") == "reversing",
+                include_terrain=args.terrain,
+            )
+            connection.request("POST", "/plan", body=json.dumps(payload))
+            response = connection.getresponse()
+            result = json.loads(response.read())
+            if response.status != 200:
+                raise RuntimeError(result)
+            report["samples"].append(
+                dict(
+                    frame=row["frame_id"],
+                    pose=payload["pose"],
+                    original=row["decision"],
+                    result=result,
+                )
+            )
+            time.sleep(0.18)
+    finally:
+        connection.close()
+        subprocess.run(["docker", "rm", "-f", identity], check=True, capture_output=True)
+        args.output.parent.mkdir(parents=True, exist_ok=True)
+        args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
+    failures = Counter(
+        v["result"].get("diagnostic", {}).get("failure", v["result"]["status"])
+        for v in report["samples"]
+    )
+    blocked = [v for v in report["samples"] if v["result"]["status"] == "blocked"]
+    print(
+        json.dumps(dict(failures=failures, first_blocked=blocked[:1], last=report["samples"][-1:]))
+    )
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/server.py

@@ -0,0 +1,449 @@
+"""Bounded local HTTP adapter for the unchanged CMU ROS 2 navigation nodes.
+
+Only simulated sensor observations enter ROS; no scene mesh or oracle route.
+The container is owned by one episode. A reset restarts all causal ROS state.
+"""
+
+import json
+import math
+import os
+import signal
+import subprocess
+import sys
+import threading
+import time
+from collections import OrderedDict
+from http.server import BaseHTTPRequestHandler, HTTPServer
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from footprint import MAX_STEP_M, regulate_command
+from geometry_msgs.msg import PointStamped, TwistStamped
+from nav_msgs.msg import Odometry
+from nav_msgs.msg import Path as RosPath
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+from sensor_msgs_py import point_cloud2
+from std_msgs.msg import Float32, Header
+from terrain_costs import TerrainCostNormalizer, underbody_support_costs
+
+
+def stamp_ns(stamp):
+    return stamp.sec * 1_000_000_000 + stamp.nanosec
+
+
+class Navigation(Node):
+    def __init__(self):
+        super().__init__("missioncore_navigation_adapter")
+        self.condition = threading.Condition()
+        self.processes = []
+        self.path = self.command = self.terrain = None
+        self.odom = self.create_publisher(Odometry, "/state_estimation", 5)
+        self.scan = self.create_publisher(PointCloud2, "/registered_scan", 5)
+        self.goal = self.create_publisher(PointStamped, "/way_point", 5)
+        self.speed = self.create_publisher(Float32, "/speed", 5)
+        self.obstacles = self.create_publisher(PointCloud2, "/added_obstacles", 5)
+        self.surface = self.create_publisher(PointCloud2, "/terrain_map", 5)
+        self.create_subscription(RosPath, "/path", self.on_path, 5)
+        self.create_subscription(TwistStamped, "/cmd_vel", self.on_command, 5)
+        self.create_subscription(PointCloud2, "/terrain_map_raw", self.on_terrain, 5)
+        self.slope_corrected = 0
+        self.terrain_processing_ms = 0.0
+        self.normalize_costs = TerrainCostNormalizer()
+        self.support_poses = OrderedDict()
+        self.underbody_corrected = 0
+        self.start_nodes()
+
+    def start_nodes(self):
+        common = dict(
+            autonomyMode=True,
+            autonomySpeed=0.3,
+            maxSpeed=1.0,
+            twoWayDrive=True,
+            joyToSpeedDelay=0.0,
+        )
+        configs = [
+            (
+                "terrain_analysis",
+                "terrainAnalysis",
+                dict(
+                    scanVoxelSize=0.06,
+                    # Keep the upstream near-field memory: an obstacle hidden
+                    # by our own chassis must not disappear after one second.
+                    decayTime=2.0,
+                    noDecayDis=4.0,
+                    useSorting=True,
+                    # Keep CMU's upstream ground quantile. Lower values make
+                    # shallow scan depressions the reference for the entire
+                    # 0.6 m neighbourhood; the median admits too much wall.
+                    quantileZ=0.25,
+                    considerDrop=True,
+                    clearDyObs=False,
+                    noDataObstacle=False,
+                    vehicleHeight=0.9,
+                    minRelZ=-2.0,
+                    maxRelZ=1.0,
+                    voxelPointUpdateThre=1,
+                    voxelTimeUpdateThre=0.0,
+                ),
+            ),
+            (
+                "local_planner",
+                "localPlanner",
+                dict(
+                    **common,
+                    pathFolder="/opt/cmu/install/local_planner/share/local_planner/paths",
+                    # Match the final monitor's 5 cm margin on every side;
+                    # otherwise CMU repeatedly proposes a forbidden corner turn.
+                    vehicleLength=1.1,
+                    vehicleWidth=1.1,
+                    useTerrainAnalysis=True,
+                    checkObstacle=True,
+                    # The pinned rectangular-filter image checks the complete
+                    # initial turn and primitive before selection. The upstream
+                    # angular wedge can wrongly exclude a clear straight escape
+                    # from an obstacle beside the rear corner.
+                    checkRotObstacle=False,
+                    adjacentRange=5.0,
+                    obstacleHeightThre=MAX_STEP_M,
+                    groundHeightThre=0.08,
+                    costHeightThre=0.08,
+                    useCost=True,
+                    pointPerPathThre=1,
+                    terrainVoxelSize=0.08,
+                    minRelZ=-0.5,
+                    maxRelZ=0.9,
+                    # Propose with a 56 cm half-width. The final swept square
+                    # check below covers front/rear corners and turning.
+                    pathScale=1.25,
+                    minPathScale=1.25,
+                    pathScaleBySpeed=False,
+                    pathRangeBySpeed=False,
+                    # Permit a safe short prefix when a full metre is obstructed.
+                    # The swept-body monitor still covers command latency and
+                    # braking; a prefix is not permission to cross its endpoint.
+                    # Upstream decrements range by 0.5 m by default, so merely
+                    # lowering the minimum skips every shorter candidate.
+                    minPathRange=0.2,
+                    pathRangeStep=0.1,
+                    dirThre=80.0,
+                    goalClearRange=0.0,
+                ),
+            ),
+            (
+                "local_planner",
+                "pathFollower",
+                dict(
+                    **common,
+                    lookAheadDis=0.7,
+                    yawRateGain=2.0,
+                    stopYawRateGain=2.0,
+                    maxYawRate=20.0,
+                    maxAccel=0.4,
+                    dirDiffThre=0.3,
+                    # The follower sees the cropped local prefix, not the
+                    # mission endpoint. Do not stop before its 0.2 m minimum;
+                    # waypoint arrival and the braking monitor remain separate.
+                    stopDisThre=0.08,
+                    slowDwnDisThre=0.7,
+                    useInclToStop=True,
+                    inclThre=30.0,
+                    stopTime=0.5,
+                    noRotAtGoal=True,
+                    pubSkipNum=0,
+                ),
+            ),
+        ]
+        for package, executable, parameters in configs:
+            args = ["ros2", "run", package, executable, "--ros-args"]
+            if executable == "terrainAnalysis":
+                args += ["-r", "/terrain_map:=/terrain_map_raw"]
+            for key, value in parameters.items():
+                args += ["-p", f"{key}:={str(value).lower() if isinstance(value, bool) else value}"]
+            self.processes.append(subprocess.Popen(args, start_new_session=True))
+
+    def stop_nodes(self):
+        for process in self.processes:
+            if process.poll() is None:
+                os.killpg(process.pid, signal.SIGTERM)
+        for process in self.processes:
+            try:
+                process.wait(timeout=3)
+            except subprocess.TimeoutExpired:
+                os.killpg(process.pid, signal.SIGKILL)
+                process.wait()
+        self.processes.clear()
+
+    def ready(self):
+        return (
+            len(self.processes) == 3
+            and all(p.poll() is None for p in self.processes)
+            and self.odom.get_subscription_count() >= 3
+            and self.scan.get_subscription_count() >= 2
+        )
+
+    def on_path(self, message):
+        with self.condition:
+            self.path = (
+                stamp_ns(message.header.stamp),
+                time.monotonic(),
+                [[p.pose.position.x, p.pose.position.y, p.pose.position.z] for p in message.poses],
+            )
+            self.condition.notify_all()
+
+    def on_command(self, message):
+        with self.condition:
+            self.command = (
+                stamp_ns(message.header.stamp),
+                time.monotonic(),
+                message.twist.linear.x,
+                message.twist.angular.z,
+            )
+            self.condition.notify_all()
+
+    def on_terrain(self, message):
+        started = time.monotonic()
+        points = point_cloud2.read_points_numpy(
+            message, field_names=["x", "y", "z", "intensity"], skip_nans=True
+        ).copy()
+        points, corrected = self.normalize_costs(points)
+        with self.condition:
+            # CMU round-trips the stamp through double seconds. Match the same
+            # sub-microsecond tolerance as the observation transaction below;
+            # never substitute an unrelated latest pose for a delayed map.
+            stamp = stamp_ns(message.header.stamp)
+            support = next(
+                (
+                    value
+                    for key, value in reversed(self.support_poses.items())
+                    if abs(key - stamp) <= 1000
+                ),
+                None,
+            )
+        underbody = 0
+        if support is not None:
+            points, underbody = underbody_support_costs(points, *support)
+        fields = [
+            PointField(name=name, offset=i * 4, datatype=PointField.FLOAT32, count=1)
+            for i, name in enumerate(("x", "y", "z", "intensity"))
+        ]
+        self.surface.publish(point_cloud2.create_cloud(message.header, fields, points))
+        with self.condition:
+            self.terrain = (stamp_ns(message.header.stamp), len(points), points)
+            self.slope_corrected = corrected
+            self.underbody_corrected = underbody
+            self.terrain_processing_ms = (time.monotonic() - started) * 1000
+            self.condition.notify_all()
+
+    def plan(self, value):
+        if not self.ready():
+            raise RuntimeError("navigation nodes are not ready")
+        points = np.asarray(value["points"], dtype=np.float32)
+        pose = np.asarray(value["pose"], dtype=np.float64)
+        goal = np.asarray(value["goal"], dtype=np.float64)
+        speed = float(value["max_speed_mps"])
+        reverse = value.get("allow_reverse", False)
+        contact_height = float(value.get("body_contact_height_m", 0.37))
+        if (
+            points.ndim != 2
+            or points.shape[1] != 3
+            or not 50 <= len(points) <= 30000
+            or pose.shape != (7,)
+            or goal.shape != (3,)
+            or not 0 <= speed <= 1
+            or not isinstance(reverse, bool)
+            or not math.isfinite(contact_height)
+            or not 0.1 <= contact_height <= 1.0
+            or not all(np.isfinite(v).all() for v in (points, pose, goal))
+            or abs(float(np.linalg.norm(pose[3:])) - 1) > 0.01
+        ):
+            raise ValueError("invalid range/odometry contract")
+        header = Header(stamp=self.get_clock().now().to_msg(), frame_id="map")
+        identity = stamp_ns(header.stamp)
+        with self.condition:
+            self.support_poses[identity] = (pose.copy(), contact_height)
+            while len(self.support_poses) > 8:
+                self.support_poses.popitem(last=False)
+        odom = Odometry(header=header, child_frame_id="vehicle")
+        odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z = map(
+            float, pose[:3]
+        )
+        q = odom.pose.pose.orientation
+        q.x, q.y, q.z, q.w = map(float, pose[3:])
+        target = PointStamped(header=header)
+        target.point.x, target.point.y, target.point.z = map(float, goal)
+        fields = [
+            PointField(name=n, offset=i * 4, datatype=PointField.FLOAT32, count=1)
+            for i, n in enumerate(("x", "y", "z", "intensity"))
+        ]
+        cloud = point_cloud2.create_cloud(
+            header, fields, np.column_stack((points, np.zeros(len(points), np.float32)))
+        )
+        # The single HTTP writer establishes one observation transaction.
+        self.goal.publish(target)
+        self.speed.publish(Float32(data=speed))
+        self.odom.publish(odom)
+        self.scan.publish(cloud)
+        with self.condition:
+            fresh = self.condition.wait_for(
+                lambda: (
+                    self.path is not None
+                    and abs(self.path[0] - identity) <= 1000
+                    and self.command is not None
+                    and abs(self.command[0] - identity) <= 1000
+                    and self.command[1] >= self.path[1]
+                    and self.terrain is not None
+                    and abs(self.terrain[0] - identity) <= 1000
+                ),
+                # R26's accumulated 26k-point map needs ~0.33 s. Returning at
+                # 0.3 s perpetually abandons each matching observation just
+                # before its terrain/path arrives. Wait for that transaction,
+                # bounded below the independent 0.8 s camera deadman. A late
+                # result is still rejected by LatestInference, never reused.
+                timeout=0.6,
+            )
+            if not fresh:
+                return {
+                    "speed_mps": 0.0,
+                    "yaw_rate_rps": 0.0,
+                    "status": "waiting-for-plan",
+                    "path": [],
+                    "pending": {
+                        "path_stamp_delta_ns": None
+                        if self.path is None
+                        else self.path[0] - identity,
+                        "command_stamp_delta_ns": None
+                        if self.command is None
+                        else self.command[0] - identity,
+                        "terrain_stamp_delta_ns": None
+                        if self.terrain is None
+                        else self.terrain[0] - identity,
+                        "path_points": None if self.path is None else len(self.path[2]),
+                        "terrain_processing_ms": self.terrain_processing_ms,
+                        "terrain_points": None if self.terrain is None else self.terrain[1],
+                    },
+                    **(
+                        {"observed_terrain": self.terrain[2].tolist()}
+                        if value.get("include_terrain") is True and self.terrain is not None
+                        else {}
+                    ),
+                }
+            path, command = self.path, self.command
+            valid = len(path[2]) > 1 and all(math.isfinite(v) for v in command[2:])
+            velocity = max(-speed, min(speed, command[2]))
+            # Reverse is admitted only by the composed recovery policy after
+            # observing full-width support. Bound heading changes to that strip.
+            direction_clear = (
+                velocity <= 0 and abs(command[3]) <= 0.15 if reverse else velocity >= 0
+            )
+            velocity, yaw_rate, command_scale = (
+                regulate_command(velocity, command[3], self.terrain[2], pose)
+                if valid and direction_clear
+                else (0.0, 0.0, 0.0)
+            )
+            footprint_clear = command_scale > 0
+            valid = valid and footprint_clear and direction_clear
+            qx, qy, qz, qw = pose[3:]
+            tilt = math.degrees(math.acos(max(-1, min(1, 1 - 2 * (qx * qx + qy * qy)))))
+            failure = (
+                "inclination"
+                if tilt >= 30
+                else "no-path"
+                if len(path[2]) <= 1
+                else "footprint"
+                if not footprint_clear
+                else "direction"
+                if not direction_clear
+                else "controller-hold"
+                if abs(command[2]) + abs(command[3]) < 1e-5
+                else "none"
+            )
+            obstacles = self.terrain[2][self.terrain[2][:, 3] > MAX_STEP_M]
+            distances = np.linalg.norm(obstacles[:, :2] - pose[:2], axis=1)
+            near = obstacles[np.argsort(distances)[:12]]
+            return {
+                "speed_mps": velocity if valid else 0.0,
+                "yaw_rate_rps": max(-0.8, min(0.8, yaw_rate)) if valid else 0.0,
+                "status": "path" if valid else "blocked",
+                "path": path[2][::3],
+                "terrain_points": self.terrain[1],
+                "footprint_clear": bool(footprint_clear),
+                "path_frame": "vehicle-yaw",
+                "diagnostic": {
+                    "failure": failure,
+                    "tilt_degrees": tilt,
+                    "controller_command": list(command[2:]),
+                    "near_obstacles": near.tolist(),
+                    "slope_corrected_points": self.slope_corrected,
+                    "underbody_support_points": self.underbody_corrected,
+                    "terrain_processing_ms": self.terrain_processing_ms,
+                    "command_scale": command_scale,
+                },
+                # Engineering replay only; this local endpoint never forwards
+                # dense geometry to the operator or changes the control input.
+                **(
+                    {"observed_terrain": self.terrain[2].tolist()}
+                    if value.get("include_terrain") is True
+                    else {}
+                ),
+            }
+
+
+def main():
+    # All ROS traffic stays inside this container. Avoid persistent Fast DDS
+    # shared-memory segments across causal node resets on Docker/WSL.
+    os.environ["FASTRTPS_DEFAULT_PROFILES_FILE"] = str(Path(__file__).with_name("fastdds.xml"))
+    rclpy.init()
+    node = Navigation()
+    thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    thread.start()
+
+    class Handler(BaseHTTPRequestHandler):
+        def reply(self, status, value):
+            body = json.dumps(value, allow_nan=False).encode()
+            self.send_response(status)
+            self.send_header("Content-Type", "application/json")
+            self.send_header("Content-Length", str(len(body)))
+            self.end_headers()
+            self.wfile.write(body)
+
+        def do_GET(self):
+            self.reply(
+                200 if self.path == "/ready" and node.ready() else 503, {"ready": node.ready()}
+            )
+
+        def do_POST(self):
+            try:
+                size = int(self.headers.get("Content-Length", "0"))
+                if not 0 < size <= 3_000_000:
+                    raise ValueError("bounded JSON body required")
+                value = json.loads(self.rfile.read(size))
+                if self.path == "/reset":
+                    node.stop_nodes()
+                    self.reply(200, {"reset": True})
+                    # Reset DDS publishers/subscribers as well as child nodes.
+                    # Replacing PID 1 preserves container ownership and clears
+                    # all cached graph/history state before the next observation.
+                    os.execv(sys.executable, [sys.executable, str(Path(__file__).resolve())])
+                elif self.path == "/plan":
+                    self.reply(200, node.plan(value))
+                else:
+                    self.reply(404, {"error": "unknown endpoint"})
+            except (ValueError, KeyError, TypeError) as exc:
+                self.reply(400, {"error": str(exc)})
+            except Exception as exc:
+                self.reply(503, {"error": str(exc)})
+
+        def log_message(self, *_):
+            pass
+
+    try:
+        HTTPServer(("0.0.0.0", 8010), Handler).serve_forever()
+    finally:
+        node.stop_nodes()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

simulation/ai-polygon/navigation/terrain_connectivity.cpp

@@ -0,0 +1,23 @@
+// Exact close-pair hazard and connectedness test; same predicates as NumPy.
+// C ABI keeps this numerical hot loop independent of ROS and Python versions.
+#include <cmath>
+#include <vector>
+template<class T> int connected_grade(const T* p, int n) {
+    if (n <= 0) return 0;
+    std::vector<int> parent(n);
+    for(int i=0;i<n;i++) parent[i]=i;
+    auto root=[&](int i) { while(parent[i]!=i) {parent[i]=parent[parent[i]];i=parent[i];} return i; };
+    for(int i=0;i<n;i++) for(int j=i+1;j<n;j++) {
+        const T dx=p[3*i]-p[3*j], dy=p[3*i+1]-p[3*j+1];
+        const T distance=std::sqrt(dx*dx+dy*dy);
+        if(distance<=T(0.12)) {
+            if(std::abs(p[3*i+2]-p[3*j+2])>T(0.1001)) return 0;
+            parent[root(i)]=root(j);
+        }
+    }
+    const int first=root(0);
+    for(int i=1;i<n;i++) if(root(i)!=first) return 0;
+    return 1;
+}
+extern "C" int terrain_connected_f32(const float* p,int n) {return connected_grade(p,n);}
+extern "C" int terrain_connected_f64(const double* p,int n) {return connected_grade(p,n);}

simulation/ai-polygon/navigation/terrain_costs.py

@@ -0,0 +1,170 @@
+"""Keep CMU height hazards, except an observed supported grade.
+
+Height above a cell's low quantile is not step height. A smooth 15-degree ramp
+can exceed 10 cm across that cell. A 6 cm voxel mesh also quantizes a continuous
+grade. Admit that surface only with broad support, a bounded plane residual and
+no observed short-range height jump exceeding the qualified 10 cm step. Vertical
+surfaces, excessive roughness and sparse/unknown support retain the CMU cost.
+"""
+
+import ctypes
+import hashlib
+import math
+from collections import OrderedDict
+from pathlib import Path
+
+import numpy as np
+
+_NATIVE_PATH = Path("/opt/missioncore/libterrain_connectivity.so")
+_NATIVE = ctypes.CDLL(str(_NATIVE_PATH)) if _NATIVE_PATH.is_file() else None
+if _NATIVE is not None:
+    for suffix, dtype in (("f32", ctypes.c_float), ("f64", ctypes.c_double)):
+        function = getattr(_NATIVE, "terrain_connected_" + suffix)
+        function.argtypes = [ctypes.POINTER(dtype), ctypes.c_int]
+        function.restype = ctypes.c_int
+
+
+def connected_grade(nearby):
+    if _NATIVE is not None and nearby.dtype in (np.dtype("float32"), np.dtype("float64")):
+        points = np.ascontiguousarray(nearby[:, :3])
+        dtype, suffix = (
+            (ctypes.c_float, "f32") if points.dtype.itemsize == 4 else (ctypes.c_double, "f64")
+        )
+        return bool(
+            getattr(_NATIVE, "terrain_connected_" + suffix)(
+                points.ctypes.data_as(ctypes.POINTER(dtype)), len(points)
+            )
+        )
+    # Reference implementation retained for portable CPU tests and comparison.
+    dx = nearby[:, None, 0] - nearby[None, :, 0]
+    dy = nearby[:, None, 1] - nearby[None, :, 1]
+    separation = np.sqrt(dx * dx + dy * dy)
+    jump = np.abs(nearby[:, None, 2] - nearby[None, :, 2])
+    if np.any((separation <= 0.12) & (jump > 0.1001)):
+        return False
+    connected = separation <= 0.12
+    reached = connected[np.argmin(np.linalg.norm(nearby[:, :2], axis=1))].copy()
+    while True:
+        expanded = np.any(connected[reached], axis=0)
+        if np.array_equal(expanded, reached):
+            return bool(reached.all())
+        reached = expanded
+
+
+def underbody_support_costs(terrain, pose, contact_height_m=0.37):
+    """Reconcile low returns already inside the current chassis footprint.
+
+    CMU's neighbourhood ground reference can label the supported terrain under
+    the chassis as a body collision. Use measured pose and the declared contact
+    height only inside the body (with a 5 cm inset), never for terrain ahead.
+    Retain drops, high returns and excessive tilt. The 8 cm band has 2 cm reserve
+    below the physically qualified 10 cm step; raw terrain memory is unchanged.
+    """
+    result = terrain.copy()
+    x, y, z, w = pose[3:]
+    up = np.array([2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)])
+    if up[2] < math.cos(math.radians(25)):
+        return result, 0
+    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
+    axes = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
+    local = (terrain[:, :2] - pose[:2]) @ axes
+    height = (terrain[:, :3] - pose[:3]) @ up + contact_height_m
+    supported = (
+        (np.abs(local) < 0.45).all(axis=1) & (np.abs(height) <= 0.08) & (terrain[:, 3] > 0.1)
+    )
+    result[supported, 3] = np.abs(height[supported])
+    return result, int(supported.sum())
+
+
+def _neighborhoods(terrain, radius=0.4):
+    """Exact radius neighborhoods without scanning the whole accumulated map.
+
+    Returns in the nine adjacent cells include every possible neighbor. Keep
+    source order so fitting and thresholds remain identical to the full scan.
+    """
+    cells = np.floor(terrain[:, :2] / radius).astype(np.int64)
+    buckets = {}
+    for index, (x, y) in enumerate(cells):
+        buckets.setdefault((x, y), []).append(index)
+    cached = {}
+
+    def around(index):
+        key = tuple(cells[index])
+        if key not in cached:
+            x, y = key
+            cached[key] = np.array(
+                sorted(
+                    i
+                    for dx in (-1, 0, 1)
+                    for dy in (-1, 0, 1)
+                    for i in buckets.get((x + dx, y + dy), ())
+                ),
+                dtype=np.int64,
+            )
+        delta = terrain[cached[key], :3] - terrain[index, :3]
+        return delta[np.linalg.norm(delta[:, :2], axis=1) <= radius]
+
+    return around
+
+
+def supported_slope_costs(terrain, cache=None):
+    result = terrain.copy()
+    if len(terrain) < 8:
+        return result, 0
+    around = _neighborhoods(terrain)
+    corrected = 0
+    for index in np.flatnonzero(terrain[:, 3] > 0.1):
+        nearby = around(index)
+        if len(nearby) < 8:
+            continue
+        # Any admitted plane spans at most a 0.8 m diameter at 25 degrees,
+        # plus the two 7.5 cm residuals. Reject tall foliage/walls before fitting.
+        if np.ptp(nearby[:, 2]) > 0.8 * math.tan(math.radians(25 + 1e-4)) + 0.15:
+            continue
+        key = None
+        if cache is not None:
+            key = hashlib.blake2b(nearby.tobytes(), digest_size=24).digest()
+            if key in cache:
+                if cache[key]:
+                    result[index, 3] = 0.0
+                    corrected += 1
+                cache.move_to_end(key)
+                continue
+            cache[key] = False
+            if len(cache) > 16384:
+                cache.popitem(last=False)
+        # No collinear strip, hidden region, multiple height layers or vertical
+        # surface is admitted as a plane. All observed points must agree.
+        covariance = np.cov(nearby[:, :2], rowvar=False)
+        if np.linalg.eigvalsh(covariance)[0] < 0.0036:
+            continue
+        matrix = np.column_stack((nearby[:, :2], np.ones(len(nearby))))
+        plane = np.linalg.lstsq(matrix, nearby[:, 2], rcond=None)[0]
+        slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
+        if not 2 <= slope <= 25 + 1e-4 or abs(plane[2]) > 0.05:
+            continue
+        if np.max(np.abs(matrix @ plane - nearby[:, 2])) > 0.075:
+            continue
+        # A permissive fit alone could erase a real ledge. Test close measured
+        # returns explicitly: even a narrow step/drop must retain its hazard.
+        if not connected_grade(nearby):
+            continue  # Do not fit a road across a gap with no returns.
+        result[index, 3] = 0.0
+        corrected += 1
+        if cache is not None:
+            cache[key] = True
+    return result, corrected
+
+
+class TerrainCostNormalizer:
+    """Reuse fits only for byte-identical observed neighborhoods, bounded in RAM.
+
+    New or changed returns always trigger a new fit. This stores no occupancy
+    belief and clears with the owning ROS node on every episode/reset.
+    """
+
+    def __init__(self):
+        self.cache = OrderedDict()
+
+    def __call__(self, terrain):
+        return supported_slope_costs(terrain, self.cache)