feat(perception): add gravity-aligned TGS evidence gate
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,205 @@
|
||||
[CmdletBinding()]
|
||||
param(
|
||||
[Parameter(Mandatory = $true)]
|
||||
[string]$ReleaseRoot,
|
||||
[Parameter(Mandatory = $true)]
|
||||
[ValidatePattern("^[A-Za-z0-9._-]{1,96}$")]
|
||||
[string]$RunId,
|
||||
[string]$SourcePackPath = "D:\NDC_MISSIONCORE\runtime\derived\e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b\lidar-pack.npz",
|
||||
[string]$OutputRoot = "D:\NDC_MISSIONCORE\runtime\results\m49-tgs-fail-closed"
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$ProgressPreference = "SilentlyContinue"
|
||||
$TravelImageTag = "ndc/mission-core-m49-t3-travel:20260826"
|
||||
$TravelImageId = "sha256:7b412020f4d8392d1d1ed1b33beadc44140f0ea8f781e62dd69796042334300f"
|
||||
$ParityImageTag = "ndc-mission-core-m48t-upstream-parity:1.9.4-cu130"
|
||||
$ParityImageId = "sha256:ceb13548617e4bd3f619766bfdff00af3fa5160946b367828da6d2233dcdcba0"
|
||||
|
||||
function Assert-LastExitCode([string]$Operation) {
|
||||
if ($LASTEXITCODE -ne 0) { throw "$Operation failed with exit code $LASTEXITCODE" }
|
||||
}
|
||||
|
||||
function Resolve-DDirectory([string]$Path, [string]$Label, [bool]$Create) {
|
||||
if ($Create -and -not (Test-Path -LiteralPath $Path)) {
|
||||
$null = New-Item -ItemType Directory -Path $Path
|
||||
}
|
||||
$item = Get-Item -LiteralPath (Resolve-Path -LiteralPath $Path).Path -Force
|
||||
if (
|
||||
-not $item.PSIsContainer -or
|
||||
($item.Attributes -band [IO.FileAttributes]::ReparsePoint) -or
|
||||
[IO.Path]::GetPathRoot($item.FullName).TrimEnd("\") -ine "D:"
|
||||
) {
|
||||
throw "$Label must be a real D: directory"
|
||||
}
|
||||
return $item.FullName
|
||||
}
|
||||
|
||||
function Resolve-DFile([string]$Path, [string]$Label) {
|
||||
$item = Get-Item -LiteralPath (Resolve-Path -LiteralPath $Path).Path -Force
|
||||
if (
|
||||
$item.PSIsContainer -or
|
||||
($item.Attributes -band [IO.FileAttributes]::ReparsePoint) -or
|
||||
[IO.Path]::GetPathRoot($item.FullName).TrimEnd("\") -ine "D:"
|
||||
) {
|
||||
throw "$Label must be a real D: file"
|
||||
}
|
||||
return $item.FullName
|
||||
}
|
||||
|
||||
function Convert-ToDockerPath([string]$Path) {
|
||||
return ($Path -replace "\\", "/")
|
||||
}
|
||||
|
||||
function Get-Container([string]$Name) {
|
||||
$rows = @(((& docker inspect $Name) | ConvertFrom-Json))
|
||||
Assert-LastExitCode "Docker inspection for $Name"
|
||||
if ($rows.Count -ne 1) { throw "Container identity for $Name is not unique" }
|
||||
return $rows[0]
|
||||
}
|
||||
|
||||
function Assert-Image([string]$Tag, [string]$ExpectedId) {
|
||||
$rows = @(((& docker image inspect $Tag) | ConvertFrom-Json))
|
||||
Assert-LastExitCode "Docker image inspection for $Tag"
|
||||
if ($rows.Count -ne 1 -or [string]$rows[0].Id -cne $ExpectedId) {
|
||||
throw "Pinned image identity changed for $Tag"
|
||||
}
|
||||
}
|
||||
|
||||
function Remove-ExactContainer([string]$Name) {
|
||||
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$Name$") {
|
||||
& docker rm --force $Name *> $null
|
||||
}
|
||||
}
|
||||
|
||||
if ($env:COMPUTERNAME -cne "DESKTOP-OPJ8J04") {
|
||||
throw "M49 TGS fail-closed evidence is pinned to Worker 006"
|
||||
}
|
||||
$release = Resolve-DDirectory $ReleaseRoot "M49 TGS release" $false
|
||||
$payload = Resolve-DDirectory (Join-Path $release "payload") "M49 TGS payload" $false
|
||||
$sourcePack = Resolve-DFile $SourcePackPath "RAVNOVES00 source pack"
|
||||
$output = Resolve-DDirectory $OutputRoot "M49 TGS output root" $true
|
||||
$runCandidate = Join-Path $output $RunId
|
||||
if (Test-Path -LiteralPath $runCandidate) { throw "M49 TGS output already exists" }
|
||||
$null = New-Item -ItemType Directory -Path $runCandidate
|
||||
$runOutput = Resolve-DDirectory $runCandidate "M49 TGS run output" $false
|
||||
|
||||
$releaseDocument = Get-Content -LiteralPath (Join-Path $payload "release.json") -Raw | ConvertFrom-Json
|
||||
if (
|
||||
$releaseDocument.schema_version -cne "missioncore.m49-tgs-worker-release/v1" -or
|
||||
$releaseDocument.worker_id -cne "worker-006" -or
|
||||
$releaseDocument.candidate_id -cne "travel-tgs-only"
|
||||
) {
|
||||
throw "M49 TGS release contract changed"
|
||||
}
|
||||
foreach ($property in $releaseDocument.files.PSObject.Properties) {
|
||||
$path = Join-Path $payload $property.Name
|
||||
$actual = (Get-FileHash -Algorithm SHA256 -LiteralPath $path).Hash.ToLowerInvariant()
|
||||
if ($actual -cne [string]$property.Value.sha256) {
|
||||
throw "M49 TGS payload digest changed: $($property.Name)"
|
||||
}
|
||||
}
|
||||
$sourcePackSha = (Get-FileHash -Algorithm SHA256 -LiteralPath $sourcePack).Hash.ToLowerInvariant()
|
||||
if ($sourcePackSha -cne "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944") {
|
||||
throw "RAVNOVES00 source pack digest changed"
|
||||
}
|
||||
$os = Get-CimInstance Win32_OperatingSystem
|
||||
$freeMemoryGiB = [double]$os.FreePhysicalMemory / 1MB
|
||||
if ($freeMemoryGiB -lt 16.0) {
|
||||
throw ("M49 TGS requires 16 GiB free memory; observed {0:N2} GiB" -f $freeMemoryGiB)
|
||||
}
|
||||
$tritonBefore = Get-Container "ndc-mission-core-triton"
|
||||
if (-not $tritonBefore.State.Running -or $tritonBefore.State.Health.Status -cne "healthy") {
|
||||
throw "Canonical Mission Core Triton must remain healthy during M49 TGS"
|
||||
}
|
||||
Assert-Image $TravelImageTag $TravelImageId
|
||||
Assert-Image $ParityImageTag $ParityImageId
|
||||
|
||||
$prepareName = "ndc-mission-core-m49-tgs-prepare-$RunId"
|
||||
$runName = "ndc-mission-core-m49-tgs-run-$RunId"
|
||||
$analyzeName = "ndc-mission-core-m49-tgs-analyze-$RunId"
|
||||
foreach ($name in @($prepareName, $runName, $analyzeName)) {
|
||||
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$name$") {
|
||||
throw "M49 TGS container name already exists: $name"
|
||||
}
|
||||
}
|
||||
|
||||
$started = [DateTimeOffset]::UtcNow
|
||||
try {
|
||||
& docker run --rm --name $prepareName --network none --cpus 8 --memory 16g `
|
||||
--entrypoint python3 `
|
||||
--volume ((Convert-ToDockerPath $sourcePack) + ":/source/lidar-pack.npz:ro") `
|
||||
--volume ((Convert-ToDockerPath $payload) + ":/release:ro") `
|
||||
--volume ((Convert-ToDockerPath $runOutput) + ":/tgs") `
|
||||
$ParityImageTag /release/prepare_tgs_fail_closed_inputs.py `
|
||||
--source-pack /source/lidar-pack.npz `
|
||||
--config /release/m49-tgs-fail-closed-evidence-v1.json `
|
||||
--output-root /tgs/inputs
|
||||
Assert-LastExitCode "M49 TGS input preparation"
|
||||
|
||||
& docker run --rm --name $runName --network none --cpus 16 --memory 24g `
|
||||
--entrypoint /bin/bash `
|
||||
--volume ((Convert-ToDockerPath $payload) + ":/release:ro") `
|
||||
--volume ((Convert-ToDockerPath $runOutput) + ":/tgs") `
|
||||
$TravelImageTag /release/run_tgs_fail_closed.sh
|
||||
Assert-LastExitCode "M49 TGS-only run"
|
||||
|
||||
& docker run --rm --name $analyzeName --network none --cpus 8 --memory 16g `
|
||||
--entrypoint python3 `
|
||||
--volume ((Convert-ToDockerPath $payload) + ":/release:ro") `
|
||||
--volume ((Convert-ToDockerPath $runOutput) + ":/tgs") `
|
||||
$ParityImageTag /release/build_tgs_fail_closed_evidence.py `
|
||||
--run-root /tgs `
|
||||
--config /release/m49-tgs-fail-closed-evidence-v1.json `
|
||||
--output-root /tgs/evidence
|
||||
Assert-LastExitCode "M49 TGS fail-closed evidence analysis"
|
||||
} finally {
|
||||
foreach ($name in @($prepareName, $runName, $analyzeName)) {
|
||||
Remove-ExactContainer $name
|
||||
}
|
||||
}
|
||||
$completed = [DateTimeOffset]::UtcNow
|
||||
|
||||
$resultPath = Join-Path $runOutput "evidence\result.json"
|
||||
if (-not (Test-Path -LiteralPath $resultPath -PathType Leaf)) {
|
||||
throw "M49 TGS evidence result is missing"
|
||||
}
|
||||
$result = Get-Content -LiteralPath $resultPath -Raw | ConvertFrom-Json
|
||||
if (
|
||||
$result.status -cne "passed" -or
|
||||
-not [bool]$result.summary.all_eligible_points_accounted -or
|
||||
[bool]$result.summary.aos_used
|
||||
) {
|
||||
throw "M49 TGS fail-closed acceptance failed"
|
||||
}
|
||||
$tritonAfter = Get-Container "ndc-mission-core-triton"
|
||||
if (
|
||||
-not $tritonAfter.State.Running -or
|
||||
$tritonAfter.State.Health.Status -cne "healthy" -or
|
||||
[string]$tritonAfter.Id -cne [string]$tritonBefore.Id
|
||||
) {
|
||||
throw "Canonical Mission Core Triton changed during M49 TGS"
|
||||
}
|
||||
$summary = [ordered]@{
|
||||
schema_version = "missioncore.m49-tgs-worker-summary/v1"
|
||||
worker_id = "worker-006"
|
||||
run_id = $RunId
|
||||
code_revision = [string]$releaseDocument.code_revision
|
||||
source_pack_sha256 = $sourcePackSha
|
||||
travel_image_id = $TravelImageId
|
||||
parity_image_id = $ParityImageId
|
||||
started_utc = $started.ToString("o")
|
||||
wall_seconds = [math]::Round(($completed - $started).TotalSeconds, 6)
|
||||
free_memory_gib_before = [math]::Round($freeMemoryGiB, 6)
|
||||
canonical_triton_id = [string]$tritonAfter.Id
|
||||
canonical_triton_health = [string]$tritonAfter.State.Health.Status
|
||||
all_eligible_points_accounted = [bool]$result.summary.all_eligible_points_accounted
|
||||
aos_used = [bool]$result.summary.aos_used
|
||||
visual_quality_accepted = $false
|
||||
realtime_accepted = $false
|
||||
navigation_or_actuation_allowed = $false
|
||||
}
|
||||
$summary | ConvertTo-Json -Depth 3 | Set-Content -LiteralPath (
|
||||
Join-Path $runOutput "worker-summary.json"
|
||||
) -Encoding utf8
|
||||
$summary | ConvertTo-Json -Depth 3
|
||||
@@ -0,0 +1,51 @@
|
||||
[CmdletBinding()]
|
||||
param(
|
||||
[Parameter(Mandatory = $true)]
|
||||
[string]$ReleaseRoot,
|
||||
[Parameter(Mandatory = $true)]
|
||||
[ValidatePattern("^[A-Za-z0-9._-]{1,96}$")]
|
||||
[string]$RunId
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$taskName = "MissionCore-M49TgsFailClosed"
|
||||
$release = (Resolve-Path -LiteralPath $ReleaseRoot).Path
|
||||
$runner = Join-Path $release "payload\Invoke-M49TgsFailClosedEvidence.ps1"
|
||||
if (-not (Test-Path -LiteralPath $runner -PathType Leaf)) {
|
||||
throw "M49 TGS runner is missing"
|
||||
}
|
||||
$existing = Get-ScheduledTask -TaskName $taskName -ErrorAction SilentlyContinue
|
||||
if ($existing -and $existing.State -eq "Running") {
|
||||
throw "$taskName is already running"
|
||||
}
|
||||
$powerShell = "$env:SystemRoot\System32\WindowsPowerShell\v1.0\powershell.exe"
|
||||
$arguments = @(
|
||||
"-NoLogo", "-NoProfile", "-NonInteractive", "-ExecutionPolicy", "Bypass",
|
||||
"-File", "`"$runner`"",
|
||||
"-ReleaseRoot", "`"$release`"",
|
||||
"-RunId", "`"$RunId`""
|
||||
) -join " "
|
||||
$userId = [System.Security.Principal.WindowsIdentity]::GetCurrent().Name
|
||||
$action = New-ScheduledTaskAction -Execute $powerShell -Argument $arguments -WorkingDirectory $release
|
||||
$principal = New-ScheduledTaskPrincipal -UserId $userId -LogonType Interactive -RunLevel Limited
|
||||
$trigger = New-ScheduledTaskTrigger -Once -At ((Get-Date).AddMinutes(30))
|
||||
$settings = New-ScheduledTaskSettingsSet `
|
||||
-AllowStartIfOnBatteries `
|
||||
-DontStopIfGoingOnBatteries `
|
||||
-StartWhenAvailable `
|
||||
-ExecutionTimeLimit ([TimeSpan]::FromHours(2))
|
||||
Register-ScheduledTask `
|
||||
-TaskName $taskName `
|
||||
-Action $action `
|
||||
-Principal $principal `
|
||||
-Trigger $trigger `
|
||||
-Settings $settings `
|
||||
-Description "One-shot CPU-only gravity-aligned TGS fail-closed evidence run." `
|
||||
-Force | Out-Null
|
||||
Start-ScheduledTask -TaskName $taskName
|
||||
[pscustomobject]@{
|
||||
task_name = $taskName
|
||||
run_id = $RunId
|
||||
release_root = $release
|
||||
state = (Get-ScheduledTask -TaskName $taskName).State.ToString()
|
||||
} | ConvertTo-Json -Compress
|
||||
@@ -0,0 +1,297 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build a fail-closed TGS evidence pack from the sealed compatibility run."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import io
|
||||
import json
|
||||
import math
|
||||
import zipfile
|
||||
from collections import Counter
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class TgsEvidenceError(RuntimeError):
|
||||
"""The sealed compatibility evidence or fail-closed contract is invalid."""
|
||||
|
||||
|
||||
def sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _load_float32(path: Path, columns: int) -> np.ndarray:
|
||||
if path.is_symlink() or not path.is_file():
|
||||
raise TgsEvidenceError(f"sealed input is unavailable: {path.name}")
|
||||
values = np.fromfile(path, dtype=np.float32)
|
||||
if values.size % columns:
|
||||
raise TgsEvidenceError(f"sealed input shape changed: {path.name}")
|
||||
result = values.reshape(-1, columns)
|
||||
if not np.isfinite(result).all():
|
||||
raise TgsEvidenceError(f"sealed input is non-finite: {path.name}")
|
||||
return result
|
||||
|
||||
|
||||
def classify_exact_input(
|
||||
native: np.ndarray,
|
||||
ground: np.ndarray,
|
||||
nonground: np.ndarray,
|
||||
*,
|
||||
min_range_m: float = 1.0,
|
||||
max_range_m: float = 80.0,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
points = np.asarray(native, dtype=np.float32)
|
||||
if points.ndim != 2 or points.shape[1:] != (4,):
|
||||
raise TgsEvidenceError("native XYZI input shape changed")
|
||||
ranges = np.linalg.norm(points[:, :2].astype(np.float64), axis=1)
|
||||
eligible = points[(ranges > min_range_m) & (ranges < max_range_m)]
|
||||
ground_counts = Counter(row[:3].tobytes() for row in ground)
|
||||
nonground_counts = Counter(row[:3].tobytes() for row in nonground)
|
||||
states = np.empty(eligible.shape[0], dtype=np.uint8)
|
||||
for index, row in enumerate(eligible):
|
||||
key = row[:3].tobytes()
|
||||
if ground_counts[key] > 0:
|
||||
states[index] = 1
|
||||
ground_counts[key] -= 1
|
||||
elif nonground_counts[key] > 0:
|
||||
states[index] = 2
|
||||
nonground_counts[key] -= 1
|
||||
else:
|
||||
states[index] = 3
|
||||
if any(value for value in ground_counts.values()) or any(
|
||||
value for value in nonground_counts.values()
|
||||
):
|
||||
raise TgsEvidenceError("TGS output is not a multiset subset of its exact input")
|
||||
if not np.isin(states, np.asarray([1, 2, 3], dtype=np.uint8)).all():
|
||||
raise TgsEvidenceError("point state reconstruction failed")
|
||||
return eligible[:, :3].copy(), states
|
||||
|
||||
|
||||
def costmap_grid(radius_m: float, cell_size_m: float) -> np.ndarray:
|
||||
if not math.isfinite(radius_m) or not math.isfinite(cell_size_m):
|
||||
raise TgsEvidenceError("costmap bounds are non-finite")
|
||||
if radius_m <= 0 or cell_size_m <= 0 or cell_size_m > radius_m:
|
||||
raise TgsEvidenceError("costmap bounds are invalid")
|
||||
minimum = math.floor(-radius_m / cell_size_m)
|
||||
maximum = math.ceil(radius_m / cell_size_m)
|
||||
cells = []
|
||||
for ix in range(minimum, maximum):
|
||||
for iy in range(minimum, maximum):
|
||||
center_x = (ix + 0.5) * cell_size_m
|
||||
center_y = (iy + 0.5) * cell_size_m
|
||||
if math.hypot(center_x, center_y) <= radius_m:
|
||||
cells.append((ix, iy, center_x, center_y))
|
||||
if not cells:
|
||||
raise TgsEvidenceError("costmap grid is empty")
|
||||
return np.asarray(cells, dtype=np.float64)
|
||||
|
||||
|
||||
def rasterize_costmap(
|
||||
points_xyz: np.ndarray,
|
||||
point_states: np.ndarray,
|
||||
grid: np.ndarray,
|
||||
*,
|
||||
cell_size_m: float,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
|
||||
lookup = {(int(row[0]), int(row[1])): index for index, row in enumerate(grid)}
|
||||
counts = np.zeros((grid.shape[0], 3), dtype=np.int32)
|
||||
minimum_z = np.full(grid.shape[0], np.nan, dtype=np.float32)
|
||||
maximum_z = np.full(grid.shape[0], np.nan, dtype=np.float32)
|
||||
for point, state in zip(points_xyz, point_states, strict=True):
|
||||
cell = (
|
||||
math.floor(float(point[0]) / cell_size_m),
|
||||
math.floor(float(point[1]) / cell_size_m),
|
||||
)
|
||||
cell_index = lookup.get(cell)
|
||||
if cell_index is None:
|
||||
continue
|
||||
counts[cell_index, int(state) - 1] += 1
|
||||
z = np.float32(point[2])
|
||||
if np.isnan(minimum_z[cell_index]) or z < minimum_z[cell_index]:
|
||||
minimum_z[cell_index] = z
|
||||
if np.isnan(maximum_z[cell_index]) or z > maximum_z[cell_index]:
|
||||
maximum_z[cell_index] = z
|
||||
states = np.zeros(grid.shape[0], dtype=np.uint8)
|
||||
states[counts[:, 0] > 0] = 1
|
||||
states[counts[:, 2] > 0] = 3
|
||||
states[counts[:, 1] > 0] = 2
|
||||
return states, counts[:, 0], counts[:, 1], counts[:, 2], np.column_stack((minimum_z, maximum_z))
|
||||
|
||||
|
||||
def _array_bytes(array: np.ndarray) -> bytes:
|
||||
stream = io.BytesIO()
|
||||
np.lib.format.write_array(stream, np.ascontiguousarray(array), allow_pickle=False)
|
||||
return stream.getvalue()
|
||||
|
||||
|
||||
def write_deterministic_npz(path: Path, arrays: dict[str, np.ndarray]) -> None:
|
||||
with zipfile.ZipFile(
|
||||
path, mode="w", compression=zipfile.ZIP_DEFLATED, compresslevel=6
|
||||
) as archive:
|
||||
for name in sorted(arrays):
|
||||
info = zipfile.ZipInfo(f"{name}.npy", date_time=(1980, 1, 1, 0, 0, 0))
|
||||
info.compress_type = zipfile.ZIP_DEFLATED
|
||||
info.external_attr = 0o100644 << 16
|
||||
archive.writestr(info, _array_bytes(arrays[name]))
|
||||
|
||||
|
||||
def build(run_root: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise TgsEvidenceError("TGS evidence output already exists")
|
||||
config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
if (
|
||||
config.get("schema_version") != "missioncore.m49-tgs-fail-closed-evidence-profile/v1"
|
||||
or config.get("invariants", {}).get("aos_allowed") is not False
|
||||
or config.get("invariants", {}).get("missing_support_means_free") is not False
|
||||
or config.get("state_codes")
|
||||
!= {
|
||||
"UNOBSERVED": 0,
|
||||
"GROUND_SUPPORT": 1,
|
||||
"NONGROUND_OCCUPIED": 2,
|
||||
"UNKNOWN_REJECTED": 3,
|
||||
}
|
||||
):
|
||||
raise TgsEvidenceError("TGS fail-closed profile changed")
|
||||
input_manifest_path = run_root / "inputs" / "input-manifest.json"
|
||||
input_manifest = json.loads(input_manifest_path.read_text(encoding="utf-8"))
|
||||
if (
|
||||
input_manifest.get("schema_version") != "missioncore.m49-tgs-fail-closed-input/v1"
|
||||
or input_manifest.get("source_pack_sha256") != config["source"]["source_pack_sha256"]
|
||||
or input_manifest.get("config_sha256") != sha256_file(config_path)
|
||||
or input_manifest.get("coordinate_frame") != "map-gravity-local"
|
||||
or input_manifest.get("future_frames_used") is not False
|
||||
or len(input_manifest.get("records", [])) != 20
|
||||
):
|
||||
raise TgsEvidenceError("sealed TGS input manifest changed")
|
||||
records = {(str(row["profile_id"]), int(row["slot"])): row for row in input_manifest["records"]}
|
||||
cell_size = float(config["costmap"]["cell_size_m"])
|
||||
radius = float(config["costmap"]["radius_m"])
|
||||
grid = costmap_grid(radius, cell_size)
|
||||
arrays: dict[str, np.ndarray] = {
|
||||
"costmap_cell_indices_xy": grid[:, :2].astype(np.int32),
|
||||
"costmap_cell_centers_xy_m": grid[:, 2:].astype(np.float32),
|
||||
}
|
||||
summaries: list[dict[str, object]] = []
|
||||
for profile in ("current_increment", "causal_rolling_1s"):
|
||||
all_points: list[np.ndarray] = []
|
||||
all_states: list[np.ndarray] = []
|
||||
offsets = [0]
|
||||
profile_grid_states: list[np.ndarray] = []
|
||||
profile_ground_counts: list[np.ndarray] = []
|
||||
profile_nonground_counts: list[np.ndarray] = []
|
||||
profile_rejected_counts: list[np.ndarray] = []
|
||||
profile_z_bounds: list[np.ndarray] = []
|
||||
for slot in range(10):
|
||||
record = records[(profile, slot)]
|
||||
native_path = run_root / "inputs" / record["relative_path"]
|
||||
if sha256_file(native_path) != record["sha256"]:
|
||||
raise TgsEvidenceError("sealed gravity-aligned input changed")
|
||||
output = run_root / "outputs" / profile
|
||||
points, states = classify_exact_input(
|
||||
_load_float32(native_path, 4),
|
||||
_load_float32(output / f"{slot}_ground.bin", 4),
|
||||
_load_float32(output / f"{slot}_nonground.bin", 4),
|
||||
)
|
||||
grid_state, ground_count, nonground_count, rejected_count, z_bounds = rasterize_costmap(
|
||||
points, states, grid, cell_size_m=cell_size
|
||||
)
|
||||
all_points.append(points.astype(np.float32, copy=False))
|
||||
all_states.append(states)
|
||||
offsets.append(offsets[-1] + points.shape[0])
|
||||
profile_grid_states.append(grid_state)
|
||||
profile_ground_counts.append(ground_count)
|
||||
profile_nonground_counts.append(nonground_count)
|
||||
profile_rejected_counts.append(rejected_count)
|
||||
profile_z_bounds.append(z_bounds)
|
||||
summaries.append(
|
||||
{
|
||||
"profile_id": profile,
|
||||
"slot": slot,
|
||||
"anchor_frame_index": int(record["anchor_frame_index"]),
|
||||
"point_count": int(points.shape[0]),
|
||||
"ground_point_count": int(np.count_nonzero(states == 1)),
|
||||
"nonground_point_count": int(np.count_nonzero(states == 2)),
|
||||
"rejected_point_count": int(np.count_nonzero(states == 3)),
|
||||
"ground_cell_count": int(np.count_nonzero(grid_state == 1)),
|
||||
"nonground_cell_count": int(np.count_nonzero(grid_state == 2)),
|
||||
"rejected_cell_count": int(np.count_nonzero(grid_state == 3)),
|
||||
"unobserved_cell_count": int(np.count_nonzero(grid_state == 0)),
|
||||
"all_points_accounted": bool(
|
||||
np.count_nonzero(states == 1)
|
||||
+ np.count_nonzero(states == 2)
|
||||
+ np.count_nonzero(states == 3)
|
||||
== points.shape[0]
|
||||
),
|
||||
}
|
||||
)
|
||||
arrays[f"{profile}_points_xyz_m"] = np.concatenate(all_points, axis=0)
|
||||
arrays[f"{profile}_point_states"] = np.concatenate(all_states, axis=0)
|
||||
arrays[f"{profile}_point_offsets"] = np.asarray(offsets, dtype=np.int64)
|
||||
arrays[f"{profile}_costmap_states"] = np.stack(profile_grid_states)
|
||||
arrays[f"{profile}_costmap_ground_point_counts"] = np.stack(profile_ground_counts)
|
||||
arrays[f"{profile}_costmap_nonground_point_counts"] = np.stack(profile_nonground_counts)
|
||||
arrays[f"{profile}_costmap_rejected_point_counts"] = np.stack(profile_rejected_counts)
|
||||
arrays[f"{profile}_costmap_z_bounds_m"] = np.stack(profile_z_bounds)
|
||||
if not all(bool(row["all_points_accounted"]) for row in summaries):
|
||||
raise TgsEvidenceError("fail-closed evidence lost an eligible input point")
|
||||
output_root.mkdir(parents=True)
|
||||
evidence_path = output_root / "evidence.npz"
|
||||
write_deterministic_npz(evidence_path, arrays)
|
||||
result = {
|
||||
"schema_version": "missioncore.m49-tgs-fail-closed-evidence-result/v1",
|
||||
"status": "passed",
|
||||
"config_sha256": sha256_file(config_path),
|
||||
"source_pack_sha256": input_manifest["source_pack_sha256"],
|
||||
"input_manifest_sha256": sha256_file(input_manifest_path),
|
||||
"evidence": {
|
||||
"path": "evidence.npz",
|
||||
"bytes": evidence_path.stat().st_size,
|
||||
"sha256": sha256_file(evidence_path),
|
||||
},
|
||||
"costmap": {
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"cell_size_m": cell_size,
|
||||
"radius_m": radius,
|
||||
"cell_count": int(grid.shape[0]),
|
||||
},
|
||||
"anchors": summaries,
|
||||
"summary": {
|
||||
"anchor_profile_count": len(summaries),
|
||||
"all_eligible_points_accounted": True,
|
||||
"aos_used": False,
|
||||
"primary_profile": "causal_rolling_1s",
|
||||
},
|
||||
"authority": {
|
||||
"visual_quality_accepted": False,
|
||||
"traversability_accepted": False,
|
||||
"realtime_accepted": False,
|
||||
"navigation_or_actuation_allowed": False,
|
||||
},
|
||||
}
|
||||
result_path_out = output_root / "result.json"
|
||||
result_path_out.write_text(
|
||||
json.dumps(result, indent=2, sort_keys=True) + "\n", encoding="utf-8"
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--run-root", type=Path, required=True)
|
||||
parser.add_argument("--config", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
arguments = parser.parse_args()
|
||||
result = build(arguments.run_root, arguments.config, arguments.output_root)
|
||||
print(json.dumps(result["summary"], indent=2, sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,183 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare gravity-aligned local clouds for the TGS-only RAVNOVES00 gate."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
SOURCE_PACK_SHA256 = "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944"
|
||||
EXPECTED_ARRAYS = {
|
||||
"cloud_offsets",
|
||||
"cloud_points_map",
|
||||
"frame_indices",
|
||||
"pose_positions_map",
|
||||
"sample_available",
|
||||
"session_seconds",
|
||||
}
|
||||
|
||||
|
||||
class TgsInputError(RuntimeError):
|
||||
"""The immutable source cannot satisfy the TGS input contract."""
|
||||
|
||||
|
||||
def sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as stream:
|
||||
for chunk in iter(lambda: stream.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def _validate_source(arrays: dict[str, np.ndarray]) -> None:
|
||||
frames = 4489
|
||||
if (
|
||||
arrays["cloud_offsets"].shape != (frames + 1,)
|
||||
or arrays["cloud_offsets"].dtype != np.int64
|
||||
or arrays["cloud_points_map"].shape != (9_207_270, 3)
|
||||
or arrays["cloud_points_map"].dtype != np.float32
|
||||
or arrays["frame_indices"].shape != (frames,)
|
||||
or arrays["frame_indices"].dtype != np.int64
|
||||
or arrays["pose_positions_map"].shape != (frames, 3)
|
||||
or arrays["pose_positions_map"].dtype != np.float64
|
||||
or arrays["sample_available"].shape != (frames,)
|
||||
or arrays["sample_available"].dtype != np.bool_
|
||||
or arrays["session_seconds"].shape != (frames,)
|
||||
or arrays["session_seconds"].dtype != np.float64
|
||||
or int(arrays["cloud_offsets"][0]) != 0
|
||||
or int(arrays["cloud_offsets"][-1]) != 9_207_270
|
||||
or np.any(np.diff(arrays["cloud_offsets"]) < 0)
|
||||
or np.any(np.diff(arrays["frame_indices"]) <= 0)
|
||||
or np.any(np.diff(arrays["session_seconds"]) < 0)
|
||||
):
|
||||
raise TgsInputError("RAVNOVES00 lidar-pack array contract changed")
|
||||
|
||||
|
||||
def _frame_points(arrays: dict[str, np.ndarray], frame_index: int) -> np.ndarray:
|
||||
start = int(arrays["cloud_offsets"][frame_index])
|
||||
stop = int(arrays["cloud_offsets"][frame_index + 1])
|
||||
return arrays["cloud_points_map"][start:stop]
|
||||
|
||||
|
||||
def gravity_local_xyzi(points_map: np.ndarray, position_map: np.ndarray) -> np.ndarray:
|
||||
points = np.asarray(points_map, dtype=np.float64)
|
||||
position = np.asarray(position_map, dtype=np.float64)
|
||||
if (
|
||||
points.ndim != 2
|
||||
or points.shape[1:] != (3,)
|
||||
or position.shape != (3,)
|
||||
or not np.isfinite(points).all()
|
||||
or not np.isfinite(position).all()
|
||||
):
|
||||
raise TgsInputError("point or position arrays are invalid")
|
||||
result = np.zeros((points.shape[0], 4), dtype=np.float32)
|
||||
result[:, :3] = (points - position).astype(np.float32)
|
||||
return result
|
||||
|
||||
|
||||
def prepare(source_pack: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise TgsInputError("TGS input output root already exists")
|
||||
if sha256_file(source_pack) != SOURCE_PACK_SHA256:
|
||||
raise TgsInputError("RAVNOVES00 lidar-pack digest changed")
|
||||
config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
if (
|
||||
config.get("schema_version") != "missioncore.m49-tgs-fail-closed-evidence-profile/v1"
|
||||
or config.get("source", {}).get("source_pack_sha256") != SOURCE_PACK_SHA256
|
||||
or config.get("source", {}).get("input_coordinate_frame")
|
||||
!= "map-gravity-local-translation-only"
|
||||
or config.get("invariants", {}).get("lidar_orientation_applied_to_tgs_input") is not False
|
||||
):
|
||||
raise TgsInputError("TGS fail-closed profile changed")
|
||||
with np.load(source_pack, allow_pickle=False) as archive:
|
||||
if not EXPECTED_ARRAYS.issubset(archive.files):
|
||||
raise TgsInputError("RAVNOVES00 lidar-pack members changed")
|
||||
arrays = {name: archive[name] for name in EXPECTED_ARRAYS}
|
||||
_validate_source(arrays)
|
||||
anchors = tuple(int(value) for value in config["anchors"])
|
||||
if len(anchors) != 10 or len(set(anchors)) != 10:
|
||||
raise TgsInputError("TGS anchors changed")
|
||||
seconds = arrays["session_seconds"]
|
||||
records: list[dict[str, object]] = []
|
||||
for profile in ("current_increment", "causal_rolling_1s"):
|
||||
for slot, anchor in enumerate(anchors):
|
||||
if not bool(arrays["sample_available"][anchor]):
|
||||
raise TgsInputError(f"anchor {anchor} has no lidar sample")
|
||||
if profile == "current_increment":
|
||||
contributors = (anchor,)
|
||||
else:
|
||||
history = float(config["profiles"][profile]["history_seconds"])
|
||||
start = int(np.searchsorted(seconds, seconds[anchor] - history, side="left"))
|
||||
contributors = tuple(
|
||||
index
|
||||
for index in range(start, anchor + 1)
|
||||
if bool(arrays["sample_available"][index])
|
||||
)
|
||||
if not contributors or contributors[-1] != anchor:
|
||||
raise TgsInputError("causal profile does not contain its anchor")
|
||||
points_map = np.concatenate(
|
||||
[_frame_points(arrays, index) for index in contributors], axis=0
|
||||
)
|
||||
if profile == "causal_rolling_1s":
|
||||
radius = float(config["profiles"][profile]["local_radius_m"])
|
||||
relative_xy = (
|
||||
points_map[:, :2].astype(np.float64) - arrays["pose_positions_map"][anchor, :2]
|
||||
)
|
||||
points_map = points_map[np.linalg.norm(relative_xy, axis=1) <= radius]
|
||||
native = gravity_local_xyzi(points_map, arrays["pose_positions_map"][anchor])
|
||||
if native.shape[0] == 0:
|
||||
raise TgsInputError("TGS profile produced an empty cloud")
|
||||
target = output_root / "profiles" / profile / "velodyne" / f"{slot:06d}.bin"
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
target.write_bytes(np.ascontiguousarray(native).tobytes())
|
||||
records.append(
|
||||
{
|
||||
"profile_id": profile,
|
||||
"slot": slot,
|
||||
"anchor_frame_index": anchor,
|
||||
"anchor_source_frame_index": int(arrays["frame_indices"][anchor]),
|
||||
"anchor_session_seconds": float(seconds[anchor]),
|
||||
"point_count": int(native.shape[0]),
|
||||
"contributing_frame_indices": list(contributors),
|
||||
"contributing_source_frame_indices": [
|
||||
int(arrays["frame_indices"][index]) for index in contributors
|
||||
],
|
||||
"relative_path": target.relative_to(output_root).as_posix(),
|
||||
"bytes": target.stat().st_size,
|
||||
"sha256": sha256_file(target),
|
||||
}
|
||||
)
|
||||
manifest = {
|
||||
"schema_version": "missioncore.m49-tgs-fail-closed-input/v1",
|
||||
"source_pack_sha256": SOURCE_PACK_SHA256,
|
||||
"config_sha256": sha256_file(config_path),
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"transform": "translation-only-preserve-map-gravity-axis",
|
||||
"intensity_policy": "zero-filled-algorithm-compatibility-only",
|
||||
"future_frames_used": False,
|
||||
"records": records,
|
||||
}
|
||||
manifest_path = output_root / "input-manifest.json"
|
||||
manifest_path.write_text(
|
||||
json.dumps(manifest, indent=2, sort_keys=True) + "\n", encoding="utf-8"
|
||||
)
|
||||
return manifest
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--source-pack", type=Path, required=True)
|
||||
parser.add_argument("--config", type=Path, required=True)
|
||||
parser.add_argument("--output-root", type=Path, required=True)
|
||||
arguments = parser.parse_args()
|
||||
manifest = prepare(arguments.source_pack, arguments.config, arguments.output_root)
|
||||
print(json.dumps({"ok": True, "records": len(manifest["records"])}, sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,95 @@
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include "travel/kitti_loader.hpp"
|
||||
#include "travel/point_types.hpp"
|
||||
#include "travel/tgs.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
void writeXYZI(
|
||||
const std::string& path,
|
||||
const travel::PointCloud<PointXYZILID>& cloud
|
||||
) {
|
||||
std::ofstream output(path, std::ios::binary);
|
||||
if (!output) {
|
||||
throw std::runtime_error("cannot open TGS output");
|
||||
}
|
||||
for (const auto& point : cloud.points) {
|
||||
const float row[4] = {point.x, point.y, point.z, point.intensity};
|
||||
output.write(reinterpret_cast<const char*>(row), sizeof(row));
|
||||
}
|
||||
if (!output) {
|
||||
throw std::runtime_error("cannot write TGS output");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc != 4) {
|
||||
std::cerr << "Usage: run_tgs_fail_closed <sequence_dir> <frame_index> <output_dir>\n";
|
||||
return 1;
|
||||
}
|
||||
const std::string sequence_dir = argv[1];
|
||||
const std::size_t frame_index = std::stoul(argv[2]);
|
||||
const std::string output_dir = argv[3];
|
||||
KittiLoader loader(sequence_dir);
|
||||
if (loader.size() == 0 || frame_index >= loader.size()) {
|
||||
std::cerr << "TGS input frame is unavailable\n";
|
||||
return 2;
|
||||
}
|
||||
auto input_xyzi = loader.cloud(frame_index);
|
||||
if (!input_xyzi) {
|
||||
std::cerr << "TGS input frame cannot be loaded\n";
|
||||
return 3;
|
||||
}
|
||||
auto input = std::make_shared<travel::PointCloud<PointXYZILID>>();
|
||||
input->reserve(input_xyzi->size());
|
||||
for (const auto& point : input_xyzi->points) {
|
||||
PointXYZILID value{};
|
||||
value.x = point.x;
|
||||
value.y = point.y;
|
||||
value.z = point.z;
|
||||
value.intensity = point.intensity;
|
||||
value.label = 0;
|
||||
value.id = 0;
|
||||
input->emplace_back(value);
|
||||
}
|
||||
|
||||
travel::TravelGroundSeg<PointXYZILID> tgs;
|
||||
tgs.setParams(
|
||||
80.0,
|
||||
1.0,
|
||||
8.0,
|
||||
3,
|
||||
5,
|
||||
10,
|
||||
0.5,
|
||||
0.125,
|
||||
0.3,
|
||||
0.940,
|
||||
200.0,
|
||||
0.03,
|
||||
0.1,
|
||||
1.0,
|
||||
true,
|
||||
false
|
||||
);
|
||||
travel::PointCloud<PointXYZILID> ground;
|
||||
travel::PointCloud<PointXYZILID> nonground;
|
||||
double tgs_seconds = 0.0;
|
||||
tgs.estimateGround(*input, ground, nonground, tgs_seconds);
|
||||
std::cout << "[TGS-ONLY] frame=" << frame_index
|
||||
<< " input=" << input->size()
|
||||
<< " ground=" << ground.size()
|
||||
<< " nonground=" << nonground.size()
|
||||
<< " tgs_seconds=" << tgs_seconds << '\n';
|
||||
const std::string base = output_dir + "/" + std::to_string(frame_index);
|
||||
writeXYZI(base + "_ground.bin", ground);
|
||||
writeXYZI(base + "_nonground.bin", nonground);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
readonly INPUT_ROOT=/tgs/inputs
|
||||
readonly OUTPUT_ROOT=/tgs/outputs
|
||||
readonly LOG_ROOT=/tgs/logs
|
||||
readonly TIMING_PATH=/tgs/tgs-timing.tsv
|
||||
readonly BINARY=/tmp/run_tgs_fail_closed
|
||||
|
||||
test -f "${INPUT_ROOT}/input-manifest.json"
|
||||
test ! -e "${OUTPUT_ROOT}"
|
||||
test ! -e "${LOG_ROOT}"
|
||||
g++ -std=c++17 -O3 -DNDEBUG \
|
||||
-I/opt/travel/src/TRAVEL/cpp/travel/core \
|
||||
-I/usr/include/eigen3 \
|
||||
/release/run_tgs_fail_closed.cpp \
|
||||
-o "${BINARY}"
|
||||
mkdir -p "${OUTPUT_ROOT}" "${LOG_ROOT}"
|
||||
printf 'profile\tslot\twall_seconds\tmax_rss_kib\n' > "${TIMING_PATH}"
|
||||
|
||||
for profile in current_increment causal_rolling_1s; do
|
||||
sequence="${INPUT_ROOT}/profiles/${profile}"
|
||||
output="${OUTPUT_ROOT}/${profile}"
|
||||
mkdir -p "${output}"
|
||||
for slot in $(seq 0 9); do
|
||||
log="${LOG_ROOT}/${profile}-${slot}.log"
|
||||
timing="${LOG_ROOT}/${profile}-${slot}.time"
|
||||
/usr/bin/time -f '%e\t%M' -o "${timing}" \
|
||||
"${BINARY}" "${sequence}" "${slot}" "${output}" > "${log}" 2>&1
|
||||
read -r wall rss < "${timing}"
|
||||
printf '%s\t%s\t%s\t%s\n' \
|
||||
"${profile}" "${slot}" "${wall}" "${rss}" >> "${TIMING_PATH}"
|
||||
test -f "${output}/${slot}_ground.bin"
|
||||
test -f "${output}/${slot}_nonground.bin"
|
||||
done
|
||||
done
|
||||
Reference in New Issue
Block a user