feat(perception): add full TGS shadow runner
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,77 @@
|
||||
{
|
||||
"schema_version": "missioncore.m49-tgs-full-shadow-profile/v1",
|
||||
"profile_id": "m49-ravnoves00-tgs-full-shadow/v1",
|
||||
"source": {
|
||||
"source_id": "RAVNOVES00",
|
||||
"session_id": "20260720T065719Z_viewer_live",
|
||||
"source_pack_id": "e10-lidar-pack-576c994a6c814e2592dd6240ace3902a5db94843312c759a73ba0c9166157d2b",
|
||||
"source_pack_sha256": "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944",
|
||||
"travel_revision": "95dc2fbd66a343efd9060c45a5711b6307a950a4",
|
||||
"input_coordinate_frame": "map-gravity-local-translation-only",
|
||||
"expected_timeline_frames": 4489,
|
||||
"expected_available_lidar_frames": 3928
|
||||
},
|
||||
"tgs": {
|
||||
"max_range_m": 80.0,
|
||||
"min_range_m": 1.0,
|
||||
"resolution_m": 8.0,
|
||||
"num_iterations": 3,
|
||||
"num_lowest_representative_points": 5,
|
||||
"minimum_points": 10,
|
||||
"seed_threshold_m": 0.5,
|
||||
"distance_threshold_m": 0.125,
|
||||
"outlier_threshold_m": 0.3,
|
||||
"normal_threshold": 0.94,
|
||||
"weight_threshold": 200.0,
|
||||
"lcc_normal_similarity": 0.03,
|
||||
"lcc_planar_distance_m": 0.1,
|
||||
"obstacle_height_m": 1.0,
|
||||
"refine_mode": true
|
||||
},
|
||||
"profile": {
|
||||
"id": "causal_rolling_1s",
|
||||
"history_seconds": 1.0,
|
||||
"local_radius_m": 12.0,
|
||||
"missing_lidar_policy": "all-cells-unobserved"
|
||||
},
|
||||
"costmap": {
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"cell_size_m": 0.45,
|
||||
"radius_m": 12.0,
|
||||
"state_priority": [
|
||||
"NONGROUND_OCCUPIED",
|
||||
"UNKNOWN_REJECTED",
|
||||
"GROUND_SUPPORT",
|
||||
"UNOBSERVED"
|
||||
]
|
||||
},
|
||||
"state_codes": {
|
||||
"UNOBSERVED": 0,
|
||||
"GROUND_SUPPORT": 1,
|
||||
"NONGROUND_OCCUPIED": 2,
|
||||
"UNKNOWN_REJECTED": 3
|
||||
},
|
||||
"acceptance": {
|
||||
"recorded_source_rate_hz": 10.0,
|
||||
"minimum_effective_timeline_fps": 10.0,
|
||||
"candidate_stage_p95_ms_max": 25.0,
|
||||
"candidate_stage_p99_ms_max": 50.0,
|
||||
"completion_age_p99_ms_max": 100.0,
|
||||
"capacity_drop_count_max": 0,
|
||||
"unaccounted_available_frame_count_max": 0,
|
||||
"unaccounted_eligible_point_count_max": 0
|
||||
},
|
||||
"invariants": {
|
||||
"all_eligible_input_points_accounted": true,
|
||||
"aos_allowed": false,
|
||||
"lidar_orientation_applied_to_tgs_input": false,
|
||||
"map_gravity_axis_preserved": true,
|
||||
"missing_support_means_free": false,
|
||||
"missing_lidar_means_unobserved": true,
|
||||
"unobserved_cells_are_emitted": true,
|
||||
"future_frames_used": false,
|
||||
"camera_projection_is_authoritative": false,
|
||||
"gpu_allowed": false,
|
||||
"navigation_or_actuation_allowed": false
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,187 @@
|
||||
[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-full-shadow"
|
||||
)
|
||||
|
||||
$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 full shadow is pinned to Worker 006" }
|
||||
$release = Resolve-DDirectory $ReleaseRoot "M49 TGS full-shadow release" $false
|
||||
$payload = Resolve-DDirectory (Join-Path $release "payload") "M49 TGS full-shadow payload" $false
|
||||
$sourcePack = Resolve-DFile $SourcePackPath "RAVNOVES00 source pack"
|
||||
$output = Resolve-DDirectory $OutputRoot "M49 TGS full-shadow output root" $true
|
||||
$runCandidate = Join-Path $output $RunId
|
||||
if (Test-Path -LiteralPath $runCandidate) { throw "M49 TGS full-shadow output already exists" }
|
||||
$null = New-Item -ItemType Directory -Path $runCandidate
|
||||
$runOutput = Resolve-DDirectory $runCandidate "M49 TGS full-shadow run output" $false
|
||||
|
||||
$releaseDocument = Get-Content -LiteralPath (Join-Path $payload "release.json") -Raw | ConvertFrom-Json
|
||||
if (
|
||||
$releaseDocument.schema_version -cne "missioncore.m49-tgs-full-shadow-worker-release/v1" -or
|
||||
$releaseDocument.worker_id -cne "worker-006" -or
|
||||
$releaseDocument.candidate_id -cne "travel-tgs-full-shadow"
|
||||
) { throw "M49 TGS full-shadow 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 full-shadow 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 24.0) {
|
||||
throw ("M49 TGS full shadow requires 24 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 full shadow"
|
||||
}
|
||||
Assert-Image $TravelImageTag $TravelImageId
|
||||
Assert-Image $ParityImageTag $ParityImageId
|
||||
|
||||
$prepareName = "ndc-mission-core-m49-tgs-full-prepare-$RunId"
|
||||
$runName = "ndc-mission-core-m49-tgs-full-run-$RunId"
|
||||
$analyzeName = "ndc-mission-core-m49-tgs-full-analyze-$RunId"
|
||||
foreach ($name in @($prepareName, $runName, $analyzeName)) {
|
||||
if (& docker ps -a --format "{{.Names}}" --filter "name=^/$name$") {
|
||||
throw "M49 TGS full-shadow 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_full_shadow_inputs.py `
|
||||
--source-pack /source/lidar-pack.npz `
|
||||
--config /release/m49-tgs-full-shadow-v1.json `
|
||||
--output-root /tgs/inputs
|
||||
Assert-LastExitCode "M49 TGS full-shadow 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_full_shadow.sh
|
||||
Assert-LastExitCode "M49 source-paced TGS full-shadow 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_full_shadow_evidence.py `
|
||||
--run-root /tgs `
|
||||
--config /release/m49-tgs-full-shadow-v1.json `
|
||||
--output-root /tgs/evidence
|
||||
Assert-LastExitCode "M49 TGS full-shadow 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 full-shadow result is missing" }
|
||||
$result = Get-Content -LiteralPath $resultPath -Raw | ConvertFrom-Json
|
||||
if (
|
||||
$result.timeline.frame_count -ne 4489 -or
|
||||
$result.timeline.available_lidar_frame_count -ne 3928 -or
|
||||
$result.timeline.missing_lidar_frame_count -ne 561 -or
|
||||
$result.point_accounting.unaccounted -ne 0
|
||||
) { throw "M49 TGS full-shadow structural 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 full shadow" }
|
||||
$summary = [ordered]@{
|
||||
schema_version = "missioncore.m49-tgs-full-shadow-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
|
||||
result_status = [string]$result.status
|
||||
all_timeline_frames_accounted = $true
|
||||
all_eligible_points_accounted = $true
|
||||
aos_used = $false
|
||||
gpu_requested = $false
|
||||
integrated_graph_performance_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,45 @@
|
||||
[CmdletBinding()]
|
||||
param(
|
||||
[Parameter(Mandatory = $true)]
|
||||
[string]$ReleaseRoot,
|
||||
[Parameter(Mandatory = $true)]
|
||||
[ValidatePattern("^[A-Za-z0-9._-]{1,96}$")]
|
||||
[string]$RunId
|
||||
)
|
||||
|
||||
$ErrorActionPreference = "Stop"
|
||||
$taskName = "MissionCore-M49TgsFullShadow"
|
||||
$release = (Resolve-Path -LiteralPath $ReleaseRoot).Path
|
||||
$runner = Join-Path $release "payload\Invoke-M49TgsFullShadow.ps1"
|
||||
if (-not (Test-Path -LiteralPath $runner -PathType Leaf)) { throw "M49 TGS full-shadow 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 source-paced TGS full shadow." `
|
||||
-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,332 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build mmap-friendly evidence for the complete source-paced TGS shadow."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from build_tgs_fail_closed_evidence import costmap_grid
|
||||
|
||||
|
||||
class FullShadowError(RuntimeError):
|
||||
"""The complete TGS shadow or its 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_xyzi(path: Path) -> np.ndarray:
|
||||
if path.is_symlink() or not path.is_file():
|
||||
raise FullShadowError(f"sealed input is unavailable: {path.name}")
|
||||
values = np.fromfile(path, dtype=np.float32)
|
||||
if values.size % 4:
|
||||
raise FullShadowError(f"sealed XYZI shape changed: {path.name}")
|
||||
result = values.reshape(-1, 4)
|
||||
if not np.isfinite(result).all():
|
||||
raise FullShadowError(f"sealed XYZI is non-finite: {path.name}")
|
||||
return result
|
||||
|
||||
|
||||
def classify_exact_input(
|
||||
native: np.ndarray, ground: np.ndarray, nonground: np.ndarray
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
ranges = np.linalg.norm(native[:, :2].astype(np.float64), axis=1)
|
||||
points = np.ascontiguousarray(native[(ranges > 1.0) & (ranges < 80.0), :3])
|
||||
output = np.ascontiguousarray(np.concatenate((ground[:, :3], nonground[:, :3]), axis=0))
|
||||
output_states = np.concatenate(
|
||||
(np.ones(ground.shape[0], dtype=np.uint8), np.full(nonground.shape[0], 2, dtype=np.uint8))
|
||||
)
|
||||
key_dtype = np.dtype((np.void, 12))
|
||||
input_keys = points.view(key_dtype).reshape(-1)
|
||||
output_keys = output.view(key_dtype).reshape(-1)
|
||||
input_order = np.argsort(input_keys, kind="stable")
|
||||
output_order = np.argsort(output_keys, kind="stable")
|
||||
sorted_input = input_keys[input_order]
|
||||
sorted_output = output_keys[output_order]
|
||||
positions = np.searchsorted(sorted_input, sorted_output, side="left")
|
||||
if sorted_output.size:
|
||||
group_starts = np.r_[0, np.flatnonzero(sorted_output[1:] != sorted_output[:-1]) + 1]
|
||||
group_lengths = np.diff(np.r_[group_starts, sorted_output.size])
|
||||
occurrence = np.arange(sorted_output.size) - np.repeat(group_starts, group_lengths)
|
||||
targets = positions + occurrence
|
||||
if (
|
||||
np.any(targets >= sorted_input.size)
|
||||
or np.any(sorted_input[targets] != sorted_output)
|
||||
or np.unique(targets).size != targets.size
|
||||
):
|
||||
raise FullShadowError("TGS output is not a multiset subset of its exact input")
|
||||
else:
|
||||
targets = np.empty(0, dtype=np.int64)
|
||||
sorted_states = np.full(points.shape[0], 3, dtype=np.uint8)
|
||||
sorted_states[targets] = output_states[output_order]
|
||||
states = np.empty_like(sorted_states)
|
||||
states[input_order] = sorted_states
|
||||
return points, states
|
||||
|
||||
|
||||
def rasterize(
|
||||
points: np.ndarray,
|
||||
states: np.ndarray,
|
||||
grid: np.ndarray,
|
||||
cell_size_m: float,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
minimum_ix = int(np.min(grid[:, 0]))
|
||||
maximum_ix = int(np.max(grid[:, 0]))
|
||||
minimum_iy = int(np.min(grid[:, 1]))
|
||||
maximum_iy = int(np.max(grid[:, 1]))
|
||||
lookup = np.full(
|
||||
(maximum_ix - minimum_ix + 1, maximum_iy - minimum_iy + 1), -1, dtype=np.int32
|
||||
)
|
||||
lookup[
|
||||
grid[:, 0].astype(np.int32) - minimum_ix,
|
||||
grid[:, 1].astype(np.int32) - minimum_iy,
|
||||
] = np.arange(grid.shape[0], dtype=np.int32)
|
||||
cell_xy = np.floor(points[:, :2] / cell_size_m).astype(np.int32)
|
||||
inside = (
|
||||
(cell_xy[:, 0] >= minimum_ix)
|
||||
& (cell_xy[:, 0] <= maximum_ix)
|
||||
& (cell_xy[:, 1] >= minimum_iy)
|
||||
& (cell_xy[:, 1] <= maximum_iy)
|
||||
)
|
||||
point_indices = np.flatnonzero(inside)
|
||||
cell_indices = lookup[
|
||||
cell_xy[inside, 0] - minimum_ix, cell_xy[inside, 1] - minimum_iy
|
||||
]
|
||||
valid = cell_indices >= 0
|
||||
point_indices = point_indices[valid]
|
||||
cell_indices = cell_indices[valid]
|
||||
cell_states = np.zeros(grid.shape[0], dtype=np.uint8)
|
||||
selected_states = states[point_indices]
|
||||
ground_cells = np.zeros(grid.shape[0], dtype=np.uint8)
|
||||
rejected_cells = np.zeros(grid.shape[0], dtype=np.uint8)
|
||||
nonground_cells = np.zeros(grid.shape[0], dtype=np.uint8)
|
||||
np.maximum.at(ground_cells, cell_indices, (selected_states == 1).astype(np.uint8))
|
||||
np.maximum.at(rejected_cells, cell_indices, (selected_states == 3).astype(np.uint8))
|
||||
np.maximum.at(nonground_cells, cell_indices, (selected_states == 2).astype(np.uint8))
|
||||
cell_states[ground_cells > 0] = 1
|
||||
cell_states[rejected_cells > 0] = 3
|
||||
cell_states[nonground_cells > 0] = 2
|
||||
minimum_z = np.full(grid.shape[0], np.inf, dtype=np.float32)
|
||||
maximum_z = np.full(grid.shape[0], -np.inf, dtype=np.float32)
|
||||
np.minimum.at(minimum_z, cell_indices, points[point_indices, 2])
|
||||
np.maximum.at(maximum_z, cell_indices, points[point_indices, 2])
|
||||
z_bounds = np.column_stack((minimum_z, maximum_z)).astype(np.float32, copy=False)
|
||||
z_bounds[~np.isfinite(z_bounds)] = np.nan
|
||||
return cell_states, z_bounds
|
||||
|
||||
|
||||
def percentile(values: np.ndarray, value: float) -> float:
|
||||
return float(np.percentile(values.astype(np.float64), value)) if values.size else 0.0
|
||||
|
||||
|
||||
def build(run_root: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise FullShadowError("full-shadow evidence output already exists")
|
||||
config = json.loads(config_path.read_text(encoding="utf-8"))
|
||||
if (
|
||||
config.get("schema_version") != "missioncore.m49-tgs-full-shadow-profile/v1"
|
||||
or config.get("invariants", {}).get("aos_allowed") is not False
|
||||
or config.get("invariants", {}).get("gpu_allowed") is not False
|
||||
or config.get("invariants", {}).get("missing_lidar_means_unobserved") is not True
|
||||
):
|
||||
raise FullShadowError("full-shadow profile changed")
|
||||
manifest_path = run_root / "inputs" / "input-manifest.json"
|
||||
manifest = json.loads(manifest_path.read_text(encoding="utf-8"))
|
||||
records = manifest.get("records", [])
|
||||
if (
|
||||
manifest.get("schema_version") != "missioncore.m49-tgs-full-shadow-input/v1"
|
||||
or manifest.get("source_pack_sha256") != config["source"]["source_pack_sha256"]
|
||||
or manifest.get("config_sha256") != sha256_file(config_path)
|
||||
or manifest.get("future_frames_used") is not False
|
||||
or len(records) != 4489
|
||||
or sum(bool(row["sample_available"]) for row in records) != 3928
|
||||
):
|
||||
raise FullShadowError("full-shadow input manifest changed")
|
||||
with (run_root / "tgs-full-timing.tsv").open("r", encoding="utf-8", newline="") as stream:
|
||||
timing_rows = list(csv.DictReader(stream, delimiter="\t"))
|
||||
if len(timing_rows) != 4489:
|
||||
raise FullShadowError("full-shadow timing frame accounting changed")
|
||||
|
||||
cell_size = float(config["costmap"]["cell_size_m"])
|
||||
radius = float(config["costmap"]["radius_m"])
|
||||
grid = costmap_grid(radius, cell_size)
|
||||
output_root.mkdir(parents=True)
|
||||
np.save(output_root / "costmap-cell-indices-xy.npy", grid[:, :2].astype(np.int32))
|
||||
np.save(output_root / "costmap-cell-centers-xy-m.npy", grid[:, 2:].astype(np.float32))
|
||||
states_out = np.lib.format.open_memmap(
|
||||
output_root / "costmap-states.npy", mode="w+", dtype=np.uint8, shape=(4489, grid.shape[0])
|
||||
)
|
||||
z_out = np.lib.format.open_memmap(
|
||||
output_root / "costmap-z-bounds-m.npy",
|
||||
mode="w+",
|
||||
dtype=np.float32,
|
||||
shape=(4489, grid.shape[0], 2),
|
||||
)
|
||||
z_out[:] = np.nan
|
||||
summaries: list[dict[str, object]] = []
|
||||
eligible_total = ground_total = nonground_total = rejected_total = 0
|
||||
available_seen = 0
|
||||
for frame_index, (record, timing) in enumerate(zip(records, timing_rows, strict=True)):
|
||||
if int(timing["timeline_frame_index"]) != frame_index:
|
||||
raise FullShadowError("full-shadow timing order changed")
|
||||
available = bool(record["sample_available"])
|
||||
if not available:
|
||||
if int(timing["sample_available"]) != 0:
|
||||
raise FullShadowError("missing LiDAR frame was processed")
|
||||
states_out[frame_index] = 0
|
||||
summaries.append(
|
||||
{
|
||||
"timeline_frame_index": frame_index,
|
||||
"source_frame_index": int(record["source_frame_index"]),
|
||||
"session_seconds": float(record["session_seconds"]),
|
||||
"sample_available": False,
|
||||
"eligible_point_count": 0,
|
||||
"ground_point_count": 0,
|
||||
"nonground_point_count": 0,
|
||||
"rejected_point_count": 0,
|
||||
"occupied_cell_count": 0,
|
||||
}
|
||||
)
|
||||
continue
|
||||
available_seen += 1
|
||||
native_path = run_root / "inputs" / str(record["relative_path"])
|
||||
if sha256_file(native_path) != record["sha256"]:
|
||||
raise FullShadowError("sealed gravity-aligned full-shadow input changed")
|
||||
output = run_root / "outputs" / "causal_rolling_1s"
|
||||
ground = load_xyzi(output / f"{frame_index}_ground.bin")
|
||||
nonground = load_xyzi(output / f"{frame_index}_nonground.bin")
|
||||
points, point_states = classify_exact_input(load_xyzi(native_path), ground, nonground)
|
||||
cell_states, z_bounds = rasterize(points, point_states, grid, cell_size)
|
||||
states_out[frame_index] = cell_states
|
||||
z_out[frame_index] = z_bounds
|
||||
ground_count = int(np.count_nonzero(point_states == 1))
|
||||
nonground_count = int(np.count_nonzero(point_states == 2))
|
||||
rejected_count = int(np.count_nonzero(point_states == 3))
|
||||
eligible_total += int(points.shape[0])
|
||||
ground_total += ground_count
|
||||
nonground_total += nonground_count
|
||||
rejected_total += rejected_count
|
||||
summaries.append(
|
||||
{
|
||||
"timeline_frame_index": frame_index,
|
||||
"source_frame_index": int(record["source_frame_index"]),
|
||||
"session_seconds": float(record["session_seconds"]),
|
||||
"sample_available": True,
|
||||
"eligible_point_count": int(points.shape[0]),
|
||||
"ground_point_count": ground_count,
|
||||
"nonground_point_count": nonground_count,
|
||||
"rejected_point_count": rejected_count,
|
||||
"occupied_cell_count": int(np.count_nonzero(cell_states == 2)),
|
||||
}
|
||||
)
|
||||
states_out.flush()
|
||||
z_out.flush()
|
||||
if available_seen != 3928 or eligible_total != ground_total + nonground_total + rejected_total:
|
||||
raise FullShadowError("full-shadow eligible point accounting failed")
|
||||
frames_path = output_root / "frames.ndjson"
|
||||
frames_path.write_text(
|
||||
"".join(json.dumps(row, sort_keys=True) + "\n" for row in summaries), encoding="utf-8"
|
||||
)
|
||||
|
||||
available_timings = [row for row in timing_rows if int(row["sample_available"]) == 1]
|
||||
tgs_ms = np.asarray([float(row["tgs_ms"]) for row in available_timings])
|
||||
completion_ms = np.asarray([float(row["completion_age_ms"]) for row in timing_rows])
|
||||
capacity_drops = sum(int(row["capacity_drop"]) for row in timing_rows)
|
||||
duration = float(records[-1]["session_seconds"]) - float(records[0]["session_seconds"])
|
||||
effective_fps = (len(records) - 1) / duration
|
||||
thresholds = config["acceptance"]
|
||||
acceptance = {
|
||||
"minimum_effective_timeline_fps": effective_fps >= float(thresholds["minimum_effective_timeline_fps"]),
|
||||
"candidate_stage_p95_ms": percentile(tgs_ms, 95) <= float(thresholds["candidate_stage_p95_ms_max"]),
|
||||
"candidate_stage_p99_ms": percentile(tgs_ms, 99) <= float(thresholds["candidate_stage_p99_ms_max"]),
|
||||
"completion_age_p99_ms": percentile(completion_ms, 99) <= float(thresholds["completion_age_p99_ms_max"]),
|
||||
"capacity_drop_count": capacity_drops <= int(thresholds["capacity_drop_count_max"]),
|
||||
"all_frames_accounted": len(summaries) == 4489 and available_seen == 3928,
|
||||
"all_eligible_points_accounted": eligible_total == ground_total + nonground_total + rejected_total,
|
||||
}
|
||||
files = {}
|
||||
for path in sorted(output_root.iterdir()):
|
||||
if path.is_file() and path.name != "result.json":
|
||||
files[path.name] = {"bytes": path.stat().st_size, "sha256": sha256_file(path)}
|
||||
result = {
|
||||
"schema_version": "missioncore.m49-tgs-full-shadow-result/v1",
|
||||
"status": "passed" if all(acceptance.values()) else "failed",
|
||||
"config_sha256": sha256_file(config_path),
|
||||
"source_pack_sha256": manifest["source_pack_sha256"],
|
||||
"input_manifest_sha256": sha256_file(manifest_path),
|
||||
"timeline": {
|
||||
"frame_count": 4489,
|
||||
"available_lidar_frame_count": 3928,
|
||||
"missing_lidar_frame_count": 561,
|
||||
"duration_seconds": duration,
|
||||
"effective_fps": effective_fps,
|
||||
},
|
||||
"costmap": {
|
||||
"coordinate_frame": "map-gravity-local",
|
||||
"cell_size_m": cell_size,
|
||||
"radius_m": radius,
|
||||
"cell_count": int(grid.shape[0]),
|
||||
},
|
||||
"point_accounting": {
|
||||
"eligible": eligible_total,
|
||||
"ground": ground_total,
|
||||
"nonground": nonground_total,
|
||||
"rejected": rejected_total,
|
||||
"unaccounted": eligible_total - ground_total - nonground_total - rejected_total,
|
||||
},
|
||||
"performance": {
|
||||
"candidate_tgs_ms": {
|
||||
"p50": percentile(tgs_ms, 50),
|
||||
"p95": percentile(tgs_ms, 95),
|
||||
"p99": percentile(tgs_ms, 99),
|
||||
"max": float(np.max(tgs_ms)),
|
||||
},
|
||||
"completion_age_ms": {
|
||||
"p50": percentile(completion_ms, 50),
|
||||
"p95": percentile(completion_ms, 95),
|
||||
"p99": percentile(completion_ms, 99),
|
||||
"max": float(np.max(completion_ms)),
|
||||
},
|
||||
"capacity_drop_count": capacity_drops,
|
||||
},
|
||||
"acceptance": acceptance,
|
||||
"files": files,
|
||||
"authority": {
|
||||
"visual_quality_accepted": False,
|
||||
"traversability_accepted": False,
|
||||
"realtime_accepted": bool(all(acceptance.values())),
|
||||
"integrated_graph_performance_accepted": False,
|
||||
"navigation_or_actuation_allowed": False,
|
||||
},
|
||||
}
|
||||
(output_root / "result.json").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({"status": result["status"], **result["performance"]}, sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,193 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare every available RAVNOVES00 frame for the source-paced TGS shadow."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import hashlib
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from prepare_tgs_fail_closed_inputs import (
|
||||
EXPECTED_ARRAYS,
|
||||
SOURCE_PACK_SHA256,
|
||||
TgsInputError,
|
||||
_frame_points,
|
||||
_validate_source,
|
||||
gravity_local_xyzi,
|
||||
sha256_file,
|
||||
)
|
||||
|
||||
FULL_SCHEMA = "missioncore.m49-tgs-full-shadow-profile/v1"
|
||||
INPUT_SCHEMA = "missioncore.m49-tgs-full-shadow-input/v1"
|
||||
TIMELINE_FRAME_COUNT = 4_489
|
||||
AVAILABLE_LIDAR_FRAME_COUNT = 3_928
|
||||
|
||||
|
||||
def _bytes_sha256(content: bytes) -> str:
|
||||
return hashlib.sha256(content).hexdigest()
|
||||
|
||||
|
||||
def prepare(source_pack: Path, config_path: Path, output_root: Path) -> dict[str, object]:
|
||||
if output_root.exists():
|
||||
raise TgsInputError("TGS full-shadow 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"))
|
||||
source = config.get("source", {})
|
||||
profile = config.get("profile", {})
|
||||
invariants = config.get("invariants", {})
|
||||
if (
|
||||
config.get("schema_version") != FULL_SCHEMA
|
||||
or source.get("source_pack_sha256") != SOURCE_PACK_SHA256
|
||||
or source.get("expected_timeline_frames") != TIMELINE_FRAME_COUNT
|
||||
or source.get("expected_available_lidar_frames") != AVAILABLE_LIDAR_FRAME_COUNT
|
||||
or source.get("input_coordinate_frame")
|
||||
!= "map-gravity-local-translation-only"
|
||||
or profile.get("id") != "causal_rolling_1s"
|
||||
or profile.get("missing_lidar_policy") != "all-cells-unobserved"
|
||||
or invariants.get("lidar_orientation_applied_to_tgs_input") is not False
|
||||
or invariants.get("future_frames_used") is not False
|
||||
or invariants.get("missing_lidar_means_unobserved") is not True
|
||||
):
|
||||
raise TgsInputError("TGS full-shadow profile changed")
|
||||
required = EXPECTED_ARRAYS | {"source_frame_indices", "pose_quaternions_map_from_lidar"}
|
||||
with np.load(source_pack, allow_pickle=False) as archive:
|
||||
if not required.issubset(archive.files):
|
||||
raise TgsInputError("RAVNOVES00 lidar-pack members changed")
|
||||
arrays = {name: archive[name] for name in required}
|
||||
_validate_source(arrays)
|
||||
if (
|
||||
arrays["source_frame_indices"].shape != (TIMELINE_FRAME_COUNT,)
|
||||
or arrays["source_frame_indices"].dtype != np.int64
|
||||
or arrays["pose_quaternions_map_from_lidar"].shape != (TIMELINE_FRAME_COUNT, 4)
|
||||
or arrays["pose_quaternions_map_from_lidar"].dtype != np.float64
|
||||
or int(np.count_nonzero(arrays["sample_available"])) != AVAILABLE_LIDAR_FRAME_COUNT
|
||||
):
|
||||
raise TgsInputError("RAVNOVES00 full timeline contract changed")
|
||||
|
||||
sequence_root = output_root / "profiles" / "causal_rolling_1s" / "velodyne"
|
||||
sequence_root.mkdir(parents=True)
|
||||
seconds = arrays["session_seconds"]
|
||||
availability = arrays["sample_available"]
|
||||
positions = arrays["pose_positions_map"]
|
||||
history_seconds = float(profile["history_seconds"])
|
||||
local_radius_m = float(profile["local_radius_m"])
|
||||
records: list[dict[str, object]] = []
|
||||
schedule_rows = [
|
||||
"timeline_frame_index\tsource_frame_index\tsession_seconds\tavailable_slot\tpoint_count"
|
||||
]
|
||||
available_slot = 0
|
||||
for frame_index in range(TIMELINE_FRAME_COUNT):
|
||||
base = {
|
||||
"timeline_frame_index": frame_index,
|
||||
"frame_index": int(arrays["frame_indices"][frame_index]),
|
||||
"source_frame_index": int(arrays["source_frame_indices"][frame_index]),
|
||||
"session_seconds": float(seconds[frame_index]),
|
||||
"position_map_m": [float(value) for value in positions[frame_index]],
|
||||
"sample_available": bool(availability[frame_index]),
|
||||
}
|
||||
if not bool(availability[frame_index]):
|
||||
records.append(
|
||||
{
|
||||
**base,
|
||||
"available_slot": None,
|
||||
"point_count": 0,
|
||||
"contributing_frame_indices": [],
|
||||
"relative_path": None,
|
||||
"bytes": 0,
|
||||
"sha256": None,
|
||||
}
|
||||
)
|
||||
schedule_rows.append(
|
||||
f"{frame_index}\t{base['source_frame_index']}\t{seconds[frame_index]:.9f}\t-1\t0"
|
||||
)
|
||||
continue
|
||||
|
||||
start = int(np.searchsorted(seconds, seconds[frame_index] - history_seconds, side="left"))
|
||||
contributors = tuple(
|
||||
index for index in range(start, frame_index + 1) if bool(availability[index])
|
||||
)
|
||||
if not contributors or contributors[-1] != frame_index:
|
||||
raise TgsInputError("causal full-shadow profile does not contain its current frame")
|
||||
points_map = np.concatenate(
|
||||
[_frame_points(arrays, index) for index in contributors], axis=0
|
||||
)
|
||||
relative_xy = points_map[:, :2].astype(np.float64) - positions[frame_index, :2]
|
||||
points_map = points_map[np.linalg.norm(relative_xy, axis=1) <= local_radius_m]
|
||||
native = gravity_local_xyzi(points_map, positions[frame_index])
|
||||
if native.shape[0] == 0:
|
||||
raise TgsInputError("available full-shadow frame produced an empty cloud")
|
||||
content = np.ascontiguousarray(native).tobytes()
|
||||
target = sequence_root / f"{available_slot:06d}.bin"
|
||||
target.write_bytes(content)
|
||||
records.append(
|
||||
{
|
||||
**base,
|
||||
"available_slot": available_slot,
|
||||
"point_count": int(native.shape[0]),
|
||||
"contributing_frame_indices": list(contributors),
|
||||
"relative_path": target.relative_to(output_root).as_posix(),
|
||||
"bytes": len(content),
|
||||
"sha256": _bytes_sha256(content),
|
||||
}
|
||||
)
|
||||
schedule_rows.append(
|
||||
f"{frame_index}\t{base['source_frame_index']}\t{seconds[frame_index]:.9f}"
|
||||
f"\t{available_slot}\t{native.shape[0]}"
|
||||
)
|
||||
available_slot += 1
|
||||
|
||||
if available_slot != AVAILABLE_LIDAR_FRAME_COUNT or len(records) != TIMELINE_FRAME_COUNT:
|
||||
raise TgsInputError("TGS full-shadow frame accounting changed")
|
||||
schedule_path = output_root / "schedule.tsv"
|
||||
schedule_path.write_text("\n".join(schedule_rows) + "\n", encoding="utf-8")
|
||||
manifest = {
|
||||
"schema_version": INPUT_SCHEMA,
|
||||
"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,
|
||||
"timeline_frame_count": TIMELINE_FRAME_COUNT,
|
||||
"available_lidar_frame_count": AVAILABLE_LIDAR_FRAME_COUNT,
|
||||
"missing_lidar_frame_count": TIMELINE_FRAME_COUNT - AVAILABLE_LIDAR_FRAME_COUNT,
|
||||
"schedule": {
|
||||
"path": "schedule.tsv",
|
||||
"bytes": schedule_path.stat().st_size,
|
||||
"sha256": sha256_file(schedule_path),
|
||||
},
|
||||
"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,
|
||||
"timeline_frames": manifest["timeline_frame_count"],
|
||||
"available_lidar_frames": manifest["available_lidar_frame_count"],
|
||||
},
|
||||
sort_keys=True,
|
||||
)
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,181 @@
|
||||
#include <chrono>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "travel/kitti_loader.hpp"
|
||||
#include "travel/point_types.hpp"
|
||||
#include "travel/tgs.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
using Clock = std::chrono::steady_clock;
|
||||
|
||||
struct ScheduleRow {
|
||||
std::size_t timeline_frame_index;
|
||||
long long source_frame_index;
|
||||
double session_seconds;
|
||||
long long available_slot;
|
||||
std::size_t point_count;
|
||||
};
|
||||
|
||||
std::vector<ScheduleRow> readSchedule(const std::string& path) {
|
||||
std::ifstream input(path);
|
||||
if (!input) {
|
||||
throw std::runtime_error("cannot open full-shadow schedule");
|
||||
}
|
||||
std::string line;
|
||||
std::getline(input, line);
|
||||
if (line != "timeline_frame_index\tsource_frame_index\tsession_seconds\tavailable_slot\tpoint_count") {
|
||||
throw std::runtime_error("full-shadow schedule header changed");
|
||||
}
|
||||
std::vector<ScheduleRow> rows;
|
||||
while (std::getline(input, line)) {
|
||||
if (line.empty()) {
|
||||
continue;
|
||||
}
|
||||
std::istringstream stream(line);
|
||||
ScheduleRow row{};
|
||||
if (!(stream >> row.timeline_frame_index >> row.source_frame_index >> row.session_seconds
|
||||
>> row.available_slot >> row.point_count)) {
|
||||
throw std::runtime_error("invalid full-shadow schedule row");
|
||||
}
|
||||
if (row.timeline_frame_index != rows.size()) {
|
||||
throw std::runtime_error("full-shadow schedule is not contiguous");
|
||||
}
|
||||
rows.push_back(row);
|
||||
}
|
||||
if (rows.size() != 4489) {
|
||||
throw std::runtime_error("full-shadow timeline frame count changed");
|
||||
}
|
||||
return rows;
|
||||
}
|
||||
|
||||
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 full-shadow 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 full-shadow TGS output");
|
||||
}
|
||||
}
|
||||
|
||||
double milliseconds(Clock::duration duration) {
|
||||
return std::chrono::duration<double, std::milli>(duration).count();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
if (argc != 5) {
|
||||
std::cerr << "Usage: run_tgs_full_shadow <sequence_dir> <schedule.tsv> <output_dir> <timing.tsv>\n";
|
||||
return 1;
|
||||
}
|
||||
try {
|
||||
const std::string sequence_dir = argv[1];
|
||||
const std::string schedule_path = argv[2];
|
||||
const std::string output_dir = argv[3];
|
||||
const std::string timing_path = argv[4];
|
||||
const auto schedule = readSchedule(schedule_path);
|
||||
KittiLoader loader(sequence_dir);
|
||||
if (loader.size() != 3928) {
|
||||
throw std::runtime_error("full-shadow available LiDAR frame count changed");
|
||||
}
|
||||
std::filesystem::create_directories(output_dir);
|
||||
std::ofstream timing(timing_path);
|
||||
if (!timing) {
|
||||
throw std::runtime_error("cannot open full-shadow timing output");
|
||||
}
|
||||
timing << "timeline_frame_index\tsource_frame_index\tsession_seconds\tsample_available"
|
||||
<< "\tavailable_slot\tinput_points\tground_points\tnonground_points"
|
||||
<< "\ttgs_ms\tstage_wall_ms\tqueue_delay_ms\tcompletion_age_ms\tcapacity_drop\n";
|
||||
timing << std::fixed << std::setprecision(6);
|
||||
|
||||
const double first_source_seconds = schedule.front().session_seconds;
|
||||
const auto run_started = Clock::now();
|
||||
std::size_t expected_slot = 0;
|
||||
for (const auto& row : schedule) {
|
||||
const auto target = run_started + std::chrono::duration_cast<Clock::duration>(
|
||||
std::chrono::duration<double>(row.session_seconds - first_source_seconds));
|
||||
const auto before_wait = Clock::now();
|
||||
if (before_wait < target) {
|
||||
std::this_thread::sleep_until(target);
|
||||
}
|
||||
const auto stage_started = Clock::now();
|
||||
const double queue_delay_ms = std::max(0.0, milliseconds(stage_started - target));
|
||||
std::size_t input_points = 0;
|
||||
std::size_t ground_points = 0;
|
||||
std::size_t nonground_points = 0;
|
||||
double tgs_seconds = 0.0;
|
||||
|
||||
if (row.available_slot >= 0) {
|
||||
if (static_cast<std::size_t>(row.available_slot) != expected_slot) {
|
||||
throw std::runtime_error("full-shadow available slot order changed");
|
||||
}
|
||||
auto input_xyzi = loader.cloud(expected_slot);
|
||||
if (!input_xyzi || input_xyzi->size() != row.point_count) {
|
||||
throw std::runtime_error("full-shadow input point count changed");
|
||||
}
|
||||
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;
|
||||
tgs.estimateGround(*input, ground, nonground, tgs_seconds);
|
||||
input_points = input->size();
|
||||
ground_points = ground.size();
|
||||
nonground_points = nonground.size();
|
||||
const std::string base = output_dir + "/" + std::to_string(row.timeline_frame_index);
|
||||
writeXYZI(base + "_ground.bin", ground);
|
||||
writeXYZI(base + "_nonground.bin", nonground);
|
||||
++expected_slot;
|
||||
}
|
||||
const auto completed = Clock::now();
|
||||
timing << row.timeline_frame_index << '\t' << row.source_frame_index << '\t'
|
||||
<< row.session_seconds << '\t' << (row.available_slot >= 0 ? 1 : 0) << '\t'
|
||||
<< row.available_slot << '\t' << input_points << '\t' << ground_points << '\t'
|
||||
<< nonground_points << '\t' << (tgs_seconds * 1000.0) << '\t'
|
||||
<< milliseconds(completed - stage_started) << '\t' << queue_delay_ms << '\t'
|
||||
<< std::max(0.0, milliseconds(completed - target)) << "\t0\n";
|
||||
if ((row.timeline_frame_index + 1) % 100 == 0) {
|
||||
timing.flush();
|
||||
std::cout << "[TGS-FULL] frame=" << (row.timeline_frame_index + 1)
|
||||
<< "/4489 available=" << expected_slot << "/3928\n";
|
||||
}
|
||||
}
|
||||
timing.flush();
|
||||
if (expected_slot != 3928) {
|
||||
throw std::runtime_error("full-shadow available frame accounting changed");
|
||||
}
|
||||
std::cout << "[TGS-FULL] complete timeline=4489 available=3928\n";
|
||||
return 0;
|
||||
} catch (const std::exception& error) {
|
||||
std::cerr << "[TGS-FULL] " << error.what() << '\n';
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
readonly INPUT_ROOT=/tgs/inputs
|
||||
readonly OUTPUT_ROOT=/tgs/outputs/causal_rolling_1s
|
||||
readonly TIMING_PATH=/tgs/tgs-full-timing.tsv
|
||||
readonly BINARY=/tmp/run_tgs_full_shadow
|
||||
|
||||
test -f "${INPUT_ROOT}/input-manifest.json"
|
||||
test -f "${INPUT_ROOT}/schedule.tsv"
|
||||
test ! -e /tgs/outputs
|
||||
test ! -e "${TIMING_PATH}"
|
||||
g++ -std=c++17 -O3 -DNDEBUG -pthread \
|
||||
-I/opt/travel/src/TRAVEL/cpp/travel/core \
|
||||
-I/usr/include/eigen3 \
|
||||
/release/run_tgs_full_shadow.cpp \
|
||||
-o "${BINARY}"
|
||||
mkdir -p "${OUTPUT_ROOT}"
|
||||
exec /usr/bin/time -v "${BINARY}" \
|
||||
"${INPUT_ROOT}/profiles/causal_rolling_1s" \
|
||||
"${INPUT_ROOT}/schedule.tsv" \
|
||||
"${OUTPUT_ROOT}" \
|
||||
"${TIMING_PATH}"
|
||||
@@ -0,0 +1,156 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build the deterministic Worker 006 release for the complete TGS shadow."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import gzip
|
||||
import hashlib
|
||||
import io
|
||||
import json
|
||||
import re
|
||||
import subprocess
|
||||
import tarfile
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
|
||||
REPOSITORY_ROOT = Path(__file__).resolve().parents[1]
|
||||
SOURCES = (
|
||||
Path("experiments/perception/worker/m49_t3_travel/prepare_tgs_fail_closed_inputs.py"),
|
||||
Path("experiments/perception/worker/m49_t3_travel/build_tgs_fail_closed_evidence.py"),
|
||||
Path("experiments/perception/worker/m49_t3_travel/prepare_tgs_full_shadow_inputs.py"),
|
||||
Path("experiments/perception/worker/m49_t3_travel/run_tgs_full_shadow.cpp"),
|
||||
Path("experiments/perception/worker/m49_t3_travel/run_tgs_full_shadow.sh"),
|
||||
Path("experiments/perception/worker/m49_t3_travel/build_tgs_full_shadow_evidence.py"),
|
||||
Path("experiments/perception/worker/Invoke-M49TgsFullShadow.ps1"),
|
||||
Path("experiments/perception/worker/Invoke-M49TgsFullShadowAsInteractiveUser.ps1"),
|
||||
Path("config/perception/m49-tgs-full-shadow-v1.json"),
|
||||
)
|
||||
PATCH_ID = re.compile(r"^[A-Za-z0-9._-]{1,96}$")
|
||||
|
||||
|
||||
class ArtifactBuildError(RuntimeError):
|
||||
"""The full-shadow release cannot be built from the declared source."""
|
||||
|
||||
|
||||
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 git_revision() -> str:
|
||||
result = subprocess.run(
|
||||
["git", "rev-parse", "HEAD"], cwd=REPOSITORY_ROOT, check=True, capture_output=True, text=True
|
||||
)
|
||||
return result.stdout.strip()
|
||||
|
||||
|
||||
def tar_info(path: Path, arcname: str) -> tarfile.TarInfo:
|
||||
info = tarfile.TarInfo(arcname)
|
||||
info.uid = info.gid = 0
|
||||
info.uname = info.gname = "root"
|
||||
info.mtime = 0
|
||||
if path.is_dir():
|
||||
info.type = tarfile.DIRTYPE
|
||||
info.mode = 0o755
|
||||
else:
|
||||
info.type = tarfile.REGTYPE
|
||||
info.mode = 0o755 if path.suffix in {".sh", ".ps1", ".py"} else 0o644
|
||||
info.size = path.stat().st_size
|
||||
return info
|
||||
|
||||
|
||||
def write_archive(stage: Path, target: Path) -> None:
|
||||
members = [stage / "manifest.env", stage / "files.txt", stage / "payload"]
|
||||
members.extend(sorted((stage / "payload").rglob("*")))
|
||||
target.parent.mkdir(parents=True, exist_ok=True)
|
||||
with (
|
||||
target.open("wb") as raw,
|
||||
gzip.GzipFile(filename="", mode="wb", fileobj=raw, mtime=0) as compressed,
|
||||
tarfile.open(fileobj=compressed, mode="w", format=tarfile.PAX_FORMAT) as archive,
|
||||
):
|
||||
for path in members:
|
||||
info = tar_info(path, path.relative_to(stage).as_posix())
|
||||
if path.is_file():
|
||||
with path.open("rb") as stream:
|
||||
archive.addfile(info, stream)
|
||||
else:
|
||||
archive.addfile(info, io.BytesIO())
|
||||
|
||||
|
||||
def build(patch_id: str, output_directory: Path, *, revision: str | None = None) -> dict[str, object]:
|
||||
if PATCH_ID.fullmatch(patch_id) is None:
|
||||
raise ArtifactBuildError("patch id is invalid")
|
||||
sources = tuple(REPOSITORY_ROOT / source for source in SOURCES)
|
||||
if any(path.is_symlink() or not path.is_file() for path in sources):
|
||||
raise ArtifactBuildError("release input is not a regular file")
|
||||
selected_revision = revision or git_revision()
|
||||
if re.fullmatch(r"[a-f0-9]{40}", selected_revision) is None:
|
||||
raise ArtifactBuildError("artifact revision is invalid")
|
||||
with tempfile.TemporaryDirectory(prefix="mission-core-m49-tgs-full-") as directory:
|
||||
stage = Path(directory)
|
||||
payload = stage / "payload"
|
||||
payload.mkdir()
|
||||
files: dict[str, dict[str, object]] = {}
|
||||
for source in sources:
|
||||
destination = payload / source.name
|
||||
destination.write_bytes(source.read_bytes())
|
||||
files[destination.name] = {"bytes": destination.stat().st_size, "sha256": sha256_file(destination)}
|
||||
release = {
|
||||
"schema_version": "missioncore.m49-tgs-full-shadow-worker-release/v1",
|
||||
"patch_id": patch_id,
|
||||
"code_revision": selected_revision,
|
||||
"worker_id": "worker-006",
|
||||
"candidate_id": "travel-tgs-full-shadow",
|
||||
"license": "GPL-3.0-or-later",
|
||||
"source_pack_sha256": "0685d24219d8236caf8b7f1685e93f6d6b59e7fd015a768d88a92bbe8b154944",
|
||||
"images": {
|
||||
"travel": "sha256:7b412020f4d8392d1d1ed1b33beadc44140f0ea8f781e62dd69796042334300f",
|
||||
"parity": "sha256:ceb13548617e4bd3f619766bfdff00af3fa5160946b367828da6d2233dcdcba0",
|
||||
},
|
||||
"authority": {
|
||||
"visual_quality_accepted": False,
|
||||
"traversability_accepted": False,
|
||||
"realtime_accepted": False,
|
||||
"integrated_graph_performance_accepted": False,
|
||||
"navigation_or_actuation_allowed": False,
|
||||
},
|
||||
"files": files,
|
||||
}
|
||||
release_path = payload / "release.json"
|
||||
release_path.write_text(json.dumps(release, indent=2, sort_keys=True) + "\n", encoding="utf-8")
|
||||
payload_names = sorted((*files, release_path.name))
|
||||
(stage / "manifest.env").write_text(
|
||||
f"id={patch_id}\ncomponent=mission-core-worker\ntype=qualification-release\n", encoding="utf-8"
|
||||
)
|
||||
(stage / "files.txt").write_text("\n".join(payload_names) + "\n", encoding="utf-8")
|
||||
target = output_directory.resolve() / f"nodedc-{patch_id}.tgz"
|
||||
write_archive(stage, target)
|
||||
return {
|
||||
"ok": True,
|
||||
"artifact": str(target),
|
||||
"sha256": sha256_file(target),
|
||||
"patch_id": patch_id,
|
||||
"code_revision": selected_revision,
|
||||
"payload_files": payload_names,
|
||||
}
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("patch_id")
|
||||
parser.add_argument("--output-directory", type=Path, default=REPOSITORY_ROOT / ".runtime/worker-artifacts")
|
||||
arguments = parser.parse_args()
|
||||
try:
|
||||
result = build(arguments.patch_id, arguments.output_directory)
|
||||
except (ArtifactBuildError, OSError, subprocess.SubprocessError) as exc:
|
||||
parser.error(str(exc))
|
||||
print(json.dumps(result, indent=2, sort_keys=True))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user