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

This commit is contained in:
DCCONSTRUCTIONS
2026-09-25 16:40:45 +03:00
parent a7c64e009d
commit f01bd39037
88 changed files with 9918 additions and 108 deletions
@@ -0,0 +1,141 @@
param(
[Parameter(Mandatory=$true)][ValidatePattern('^[a-f0-9]{16}$')][string]$Release,
[switch]$Remove
)
$ErrorActionPreference='Stop'
$ProgressPreference='SilentlyContinue'
[Console]::OutputEncoding=[System.Text.Encoding]::UTF8
$root='D:\NDC_MISSIONCORE\runtime\simulation'
$taskName='ndc-ai-polygon-worker'
$isaac=Join-Path $root 'isaac-sim-6.1.0'
$python=Join-Path $isaac 'kit\python\python.exe'
$worker=Join-Path $root "releases\ai-polygon-$Release\simulation\ai-polygon\realtime_worker.py"
$firewallBackup=Join-Path $root 'private\stream-firewall-backup.json'
if (Test-Path (Join-Path $root 'state\active.json')) {
throw 'Finish or reconcile the active episode before changing the service.'
}
if ($Remove) {
if (Get-ScheduledTask -TaskName $taskName -ErrorAction SilentlyContinue) {
Stop-ScheduledTask -TaskName $taskName
Unregister-ScheduledTask -TaskName $taskName -Confirm:$false
}
foreach ($name in @('ndc-ai-polygon-signal','ndc-ai-polygon-media')) {
Get-NetFirewallRule -Name $name -ErrorAction SilentlyContinue | Remove-NetFirewallRule
}
if (Test-Path $firewallBackup) {
foreach ($rule in (Get-Content $firewallBackup -Raw | ConvertFrom-Json)) {
Get-NetFirewallRule -Name $rule.Name -ErrorAction Stop | Set-NetFirewallRule -Action Block -LocalPort $rule.LocalPort
Get-NetFirewallRule -Name $rule.ExceptionName -ErrorAction SilentlyContinue | Remove-NetFirewallRule
}
}
return
}
foreach ($file in @($python,$worker,(Join-Path $root 'private\worker.token'))) {
if (!(Test-Path $file)) { throw "Missing prepared payload: $file" }
}
$payloadRoot=Join-Path $root "releases\ai-polygon-$Release"
$manifestPath=Join-Path $root "releases\ai-polygon-$Release.manifest.json"
if (!(Get-FileHash $manifestPath -Algorithm SHA256).Hash.ToLower().StartsWith($Release)) {
throw 'Manifest identity mismatch'
}
$manifest=Get-Content $manifestPath -Raw | ConvertFrom-Json
$verified=0
foreach($entry in $manifest.PSObject.Properties) {
$file=[IO.Path]::GetFullPath((Join-Path $payloadRoot $entry.Name))
if (!$file.StartsWith($payloadRoot+'\',[StringComparison]::OrdinalIgnoreCase) -or
$entry.Value -notmatch '^[a-f0-9]{64}$' -or
(Get-FileHash $file -Algorithm SHA256).Hash.ToLower() -ne $entry.Value) {
throw "Payload identity mismatch: $($entry.Name)"
}
$verified++
}
if($verified -lt 1) { throw 'Empty release manifest' }
$address=(& 'C:\Program Files\Tailscale\tailscale.exe' ip -4).Trim()
$clientAddress=($env:SSH_CONNECTION -split ' ')[0]
foreach ($value in @($address,$clientAddress)) {
$ip=[Net.IPAddress]::Parse($value)
$bytes=$ip.GetAddressBytes()
if ($bytes.Length -ne 4 -or $bytes[0] -ne 100 -or $bytes[1] -lt 64 -or $bytes[1] -gt 127) {
throw 'This installer admits only the existing private Tailscale path.'
}
}
$kit=Join-Path $isaac 'kit\python\kit.exe'
# Windows' user-prompt Block rules override scoped Allow rules. Keep those rules
# blocking every other port; for the two admitted ports retain an explicit Block
# for every remote address EXCEPT the current private operator. Preserve rollback.
$oldRules=@()
if (Test-Path $firewallBackup) { $oldRules=@(Get-Content $firewallBackup -Raw | ConvertFrom-Json) }
$clientBytes=([Net.IPAddress]::Parse($clientAddress)).GetAddressBytes()
# Windows rejects prefix /0 here. These disjoint CIDRs are the exact IPv4
# complement of the operator /32, plus both IPv6 halves. No address is exposed.
$number=[uint64]$clientBytes[0]*16777216+[uint64]$clientBytes[1]*65536+[uint64]$clientBytes[2]*256+$clientBytes[3]
$excluded=@('::/1','8000::/1')
for($prefix=1;$prefix -le 32;$prefix++) {
$bit=[uint64][math]::Pow(2,32-$prefix)
$network=([uint64]([math]::Floor($number/$bit)*$bit)) -bxor $bit
$networkBytes=[byte[]]@((($network -shr 24) -band 255),(($network -shr 16) -band 255),(($network -shr 8) -band 255),($network -band 255))
$excluded+="$([Net.IPAddress]::new($networkBytes))/$prefix"
}
try {
$blocked=Get-NetFirewallRule | Where-Object {
$_.Enabled -eq 'True' -and $_.Direction -eq 'Inbound' -and
($_.Action -eq 'Block' -or $_.Name -in @($oldRules.Name)) -and
$_.Name -notlike 'ndc-*' -and (($_ | Get-NetFirewallApplicationFilter).Program -ieq $kit)
}
foreach($rule in $blocked) {
$filter=$rule | Get-NetFirewallPortFilter
$port=if($filter.Protocol -eq 'TCP'){49100}elseif($filter.Protocol -eq 'UDP'){47998}else{throw 'Unexpected Isaac block protocol'}
$exceptionName="ndc-ai-polygon-block-other-$($filter.Protocol.ToLower())"
$backup=$oldRules | Where-Object {$_.Name -eq $rule.Name}
if(!$backup) {
if($filter.LocalPort -ne 'Any' -or ($rule | Get-NetFirewallAddressFilter).RemoteAddress -ne 'Any') {
throw 'Inspect a nonstandard Isaac firewall policy before migrating it.'
}
$oldRules+=@{Name=$rule.Name;LocalPort='Any';ExceptionName=$exceptionName}
$oldRules | ConvertTo-Json -Depth 4 | Set-Content -Encoding UTF8 $firewallBackup
}
Get-NetFirewallRule -Name $exceptionName -ErrorAction SilentlyContinue | Remove-NetFirewallRule
New-NetFirewallRule -Name $exceptionName -DisplayName $exceptionName -Group 'ndc-ai-polygon' `
-Direction Inbound -Action Block -Program $kit -Protocol $filter.Protocol -LocalPort $port `
-RemoteAddress $excluded -Profile $rule.Profile | Out-Null
$rule | Set-NetFirewallRule -Action Block -LocalPort @("1-$($port-1)","$($port+1)-65535")
}
foreach ($spec in @(@('ndc-ai-polygon-signal','TCP',49100),@('ndc-ai-polygon-media','UDP',47998))) {
$existing=Get-NetFirewallRule -Name $spec[0] -ErrorAction SilentlyContinue
if ($existing) { $existing | Remove-NetFirewallRule }
New-NetFirewallRule -Name $spec[0] -DisplayName $spec[0] -Group 'ndc-ai-polygon' `
-Direction Inbound -Action Allow -Program $kit -Protocol $spec[1] -LocalPort $spec[2] `
-LocalAddress $address -RemoteAddress $clientAddress -Profile Any | Out-Null
}
} catch {
foreach($old in $oldRules) {
Get-NetFirewallRule -Name $old.Name -ErrorAction Stop | Set-NetFirewallRule -Action Block -LocalPort $old.LocalPort
Get-NetFirewallRule -Name $old.ExceptionName -ErrorAction SilentlyContinue | Remove-NetFirewallRule
}
foreach($name in @('ndc-ai-polygon-signal','ndc-ai-polygon-media')) {
Get-NetFirewallRule -Name $name -ErrorAction SilentlyContinue | Remove-NetFirewallRule
}
throw
}
if (Get-ScheduledTask -TaskName $taskName -ErrorAction SilentlyContinue) {
Stop-ScheduledTask -TaskName $taskName
}
$arguments="-u `"$worker`" --core http://127.0.0.1:18080 --token-file `"$root\private\worker.token`" --state `"$root\state`" --isaac `"$isaac`" --stream-address $address"
$action=New-ScheduledTaskAction -Execute $python -Argument $arguments -WorkingDirectory $root
$user=[Security.Principal.WindowsIdentity]::GetCurrent().Name
$principal=New-ScheduledTaskPrincipal -UserId $user -LogonType Interactive -RunLevel Limited
$trigger=New-ScheduledTaskTrigger -AtLogOn -User $user
$settings=New-ScheduledTaskSettingsSet -MultipleInstances IgnoreNew -RestartCount 3 `
-RestartInterval (New-TimeSpan -Minutes 1) -ExecutionTimeLimit ([TimeSpan]::Zero) `
-AllowStartIfOnBatteries -DontStopIfGoingOnBatteries -StartWhenAvailable
Register-ScheduledTask -TaskName $taskName -Action $action -Principal $principal `
-Trigger $trigger -Settings $settings -Force | Out-Null
Start-ScheduledTask -TaskName $taskName
[ordered]@{schema_version='missioncore.ai-polygon-service/v1'; release=$Release;
verified_files=$verified;
installed_at=[DateTime]::UtcNow.ToString('o'); task=$taskName;
lifecycle='Windows task, at user logon; independent of SSH/viewer';
control='existing operator tunnel 18080'; video='private Tailscale only';
rollback='Install-RealtimeWorker.ps1 -Release <previous> or -Remove; requires no active episode'
} | ConvertTo-Json | Set-Content -Encoding UTF8 (Join-Path $root 'realtime-service-installation.json')
Get-Content (Join-Path $root 'realtime-service-installation.json')
@@ -0,0 +1,38 @@
param([switch]$Extract)
$ErrorActionPreference = 'Stop'
$ProgressPreference = 'SilentlyContinue'
# Additive workstation package; never changes Docker, drivers, PATH or services.
$root = 'D:\NDC_MISSIONCORE\runtime\simulation'
$archive = Join-Path $root 'isaac-sim-standalone-6.1.0-windows-x86_64.zip'
$target = Join-Path $root 'isaac-sim-6.1.0'
$url = 'https://downloads.isaacsim.nvidia.com/isaac-sim-standalone-6.1.0-windows-x86_64.zip'
$vendorMd5 = 'a07968e980072c9ca27b2166443e2d89'
New-Item -ItemType Directory -Force $root | Out-Null
if ((Get-PSDrive D).Free -lt 60GB) { throw 'Isaac preparation requires 60 GiB free on D:' }
if (!(Test-Path $archive)) {
& curl.exe --fail --location --retry 3 --connect-timeout 20 --max-time 7200 --continue-at - --output "$archive.part" $url
if ($LASTEXITCODE -ne 0) { throw 'Download interrupted; partial archive retained for resume.' }
if ((Get-FileHash "$archive.part" -Algorithm MD5).Hash.ToLower() -ne $vendorMd5) {
throw 'Archive does not match the NVIDIA download-page checksum.'
}
Move-Item "$archive.part" $archive
}
if ((Get-FileHash $archive -Algorithm MD5).Hash.ToLower() -ne $vendorMd5) { throw 'Archive checksum changed.' }
if ($Extract -and !(Test-Path (Join-Path $target 'python.bat'))) {
if (Test-Path $target) { throw 'Incomplete target exists; inspect it before resuming extraction.' }
New-Item -ItemType Directory $target | Out-Null
& tar.exe -xf $archive -C $target
if ($LASTEXITCODE -ne 0) { throw 'Extraction failed; files retained for diagnosis.' }
}
[ordered]@{
schema_version = 'missioncore.ai-polygon-isaac-preparation/v1'
prepared_at = [DateTime]::UtcNow.ToString('o')
source_url = $url
vendor_md5 = $vendorMd5
sha256 = (Get-FileHash $archive -Algorithm SHA256).Hash.ToLower()
byte_length = (Get-Item $archive).Length
target = $target
extracted = (Test-Path (Join-Path $target 'python.bat'))
launched = $false
} | ConvertTo-Json | Set-Content -Encoding UTF8 (Join-Path $root 'isaac-6.1.0-preparation.json')
Get-Content (Join-Path $root 'isaac-6.1.0-preparation.json')
@@ -0,0 +1,33 @@
$ErrorActionPreference = 'Stop'
$ProgressPreference = 'SilentlyContinue'
$root = 'D:\NDC_MISSIONCORE\runtime\simulation\assets\jetbot-6.1-v1'
$origin = 'https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/Robots_Multiphysics/NVIDIA/Jetbot/'
$files = @('jetbot.usda', 'payloads/base.usda', 'payloads/instances.usda', 'payloads/robot.usda', 'payloads/materials.usda', 'payloads/geometries.usd', 'payloads/Physics/physics.usda', 'payloads/Physics/physx.usda')
$manifest = Join-Path $root 'asset-manifest.json'
if (Test-Path $manifest) {
$existing = Get-Content $manifest -Raw | ConvertFrom-Json
foreach ($file in $existing.files) {
if ((Get-FileHash (Join-Path $root $file.path) -Algorithm SHA256).Hash.ToLower() -ne $file.sha256) { throw 'Prepared Jetbot asset changed' }
}
Get-Content $manifest
exit 0
}
New-Item -ItemType Directory $root -Force | Out-Null
$rows = @()
$total = 0
foreach ($relative in $files) {
$target = Join-Path $root $relative
New-Item -ItemType Directory (Split-Path $target) -Force | Out-Null
if (!(Test-Path $target)) {
& curl.exe --fail --silent --show-error --location --connect-timeout 10 --max-time 180 --max-filesize 536870912 --retry 2 --continue-at - --output "$target.part" ($origin + $relative)
if ($LASTEXITCODE -ne 0) { throw "Jetbot download failed: $relative" }
Move-Item "$target.part" $target
}
$length = (Get-Item $target).Length
$total += $length
if ($total -gt 536870912) { throw 'Jetbot package exceeds 512 MiB budget' }
$rows += [ordered]@{path=$relative; source_url=($origin+$relative); byte_length=$length; sha256=(Get-FileHash $target -Algorithm SHA256).Hash.ToLower()}
}
# OmniPBR.mdl and OmniGlass.mdl are bundled renderer materials, not remote assets.
[ordered]@{schema_version='missioncore.ai-polygon-robot-assets/v1'; version='isaac-6.1-jetbot'; files=$rows; byte_length=$total; prepared_at=[DateTime]::UtcNow.ToString('o')} | ConvertTo-Json -Depth 5 | Set-Content -Encoding UTF8 $manifest
Get-Content $manifest
+16
View File
@@ -0,0 +1,16 @@
param([Parameter(Mandatory=$true)][string]$InputManifest,
[Parameter(Mandatory=$true)][string]$OutputPly,
[int]$Lod = -1)
$ErrorActionPreference = 'Stop'
$ProgressPreference = 'SilentlyContinue'
$tool = 'D:\NDC_MISSIONCORE\runtime\simulation\tools\splat-transform-3.4.0\node_modules\@playcanvas\splat-transform\bin\cli.mjs'
if (!(Test-Path $InputManifest)) { throw 'Source archive missing' }
if (Test-Path $OutputPly) { throw 'Do not overwrite a prepared scene' }
$tmp = $OutputPly -replace '\.ply$', '.part.ply'
if ($tmp -eq $OutputPly -or (Test-Path $tmp)) { throw 'Invalid or unfinished output' }
$options = @('--memory', '--no-tty')
if ($Lod -ge 0) { $options += @('--select-lod', [string]$Lod) }
& node.exe $tool @options $InputManifest $tmp
if ($LASTEXITCODE -ne 0) { throw 'Scene conversion failed' }
Move-Item $tmp $OutputPly
@{file=$OutputPly;bytes=(Get-Item $OutputPly).Length;sha256=(Get-FileHash $OutputPly -Algorithm SHA256).Hash.ToLower()} | ConvertTo-Json
@@ -0,0 +1,19 @@
$ErrorActionPreference = 'Stop'
$ProgressPreference = 'SilentlyContinue'
$root = 'D:\NDC_MISSIONCORE\runtime\simulation\tools\splat-transform-3.4.0'
$source = Join-Path $PSScriptRoot 'splat-tools'
if ([int]((& node.exe --version).TrimStart('v').Split('.')[0]) -lt 22) { throw 'Node 22 or newer required' }
New-Item -ItemType Directory $root -Force | Out-Null
foreach ($name in @('package.json', 'package-lock.json')) {
$target = Join-Path $root $name
if (Test-Path $target) {
if ((Get-FileHash $target).Hash -ne (Get-FileHash (Join-Path $source $name)).Hash) { throw 'Splat tools definition changed' }
} else { Copy-Item (Join-Path $source $name) $target }
}
Push-Location $root
try {
& npm.cmd ci --ignore-scripts --no-audit --no-fund --cache "$root\npm-cache"
if ($LASTEXITCODE -ne 0) { throw 'Splat tools preparation failed' }
& node.exe "$root\node_modules\@playcanvas\splat-transform\bin\cli.mjs" --version
if ($LASTEXITCODE -ne 0) { throw 'Splat tools validation failed' }
} finally { Pop-Location }
+40
View File
@@ -0,0 +1,40 @@
"""Package current source plus simulation adapters into a digest-addressed archive."""
import argparse
import hashlib
import json
import tarfile
from pathlib import Path
root = Path(__file__).resolve().parents[2]
parser = argparse.ArgumentParser()
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=True)
files = sorted(
p
for folder in (root / "src", root / "simulation/ai-polygon")
for p in folder.rglob("*")
if p.is_file() and not p.is_symlink() and "__pycache__" not in p.parts
)
manifest = {
p.relative_to(root).as_posix(): hashlib.sha256(p.read_bytes()).hexdigest() for p in files
}
encoded = json.dumps(manifest, sort_keys=True, indent=2).encode()
identity = hashlib.sha256(encoded).hexdigest()
path = args.output / ("ai-polygon-" + identity[:16] + ".tgz")
with tarfile.open(path, "w:gz") as archive:
for p in files:
archive.add(p, arcname=p.relative_to(root).as_posix(), recursive=False)
manifest_path = path.with_suffix(".manifest.json")
manifest_path.write_bytes(encoded)
print(
json.dumps(
{
"bundle": str(path),
"identity": identity,
"sha256": hashlib.sha256(path.read_bytes()).hexdigest(),
"files": len(files),
}
)
)
+83
View File
@@ -0,0 +1,83 @@
name: ndc-ai-polygon-models
services:
segformer:
image: sha256:f37681beb9feecee991e71a60a68259f15b56b168c939db3c3cb259c322e9536
pull_policy: never
container_name: ndc-ai-polygon-segformer
restart: "no"
gpus: all
cpus: 3
mem_limit: 4g
ports: ["127.0.0.1:18091:8010"]
volumes:
- type: bind
source: ./segformer
target: /adapter/segformer
read_only: true
- type: bind
source: D:/NDC_MISSIONCORE/runtime/simulation/assets/segformer-b2-ade/de01bae28967510f9ddd496c60a969357195400c
target: /assets
read_only: true
labels:
com.nodedc.product: mission-core
com.nodedc.stack: ai-polygon
com.nodedc.role: segmentation
com.nodedc.managed-by: ai-polygon-worker
ddrnet:
image: sha256:a3b7d22f5d3bfdf2d84444b936c8b01abf8243be652387d7e2024ba7bda587f5
pull_policy: never
container_name: ndc-ai-polygon-ddrnet
restart: "no"
gpus: all
shm_size: 1gb
ports: ["127.0.0.1:18091:8010"]
entrypoint: ["/opt/conda/envs/goose/bin/python", "-B", "/adapter/ddrnet_server.py"]
volumes:
- type: bind
source: ./ddrnet_server.py
target: /adapter/ddrnet_server.py
read_only: true
- type: bind
source: D:/NDC_MISSIONCORE/runtime/assets/observatory-portable/lab-v1-static-files-v1
target: /assets
read_only: true
labels:
com.nodedc.product: mission-core
com.nodedc.stack: ai-polygon
com.nodedc.role: segmentation
com.nodedc.managed-by: ai-polygon-worker
detector:
image: sha256:58df7489c3f2276f9591d500a012dee03e23d35543ce3c390b4c001e6bf90794
pull_policy: never
container_name: ndc-ai-polygon-rfdetr
restart: "no"
gpus: all
shm_size: 1gb
ports: ["127.0.0.1:18092:8000"]
entrypoint: ["tritonserver"]
command:
- --model-repository=/models
- --model-control-mode=explicit
- --load-model=rf_detr_large
- --allow-grpc=false
- --allow-metrics=false
- --pinned-memory-pool-byte-size=16777216
- --cuda-memory-pool-byte-size=0:16777216
volumes:
- type: bind
source: D:/NDC_MISSIONCORE/runtime/experiments/m48t-fixed-detector-20260825T095425Z/triton-models/rf_detr_large
target: /models/rf_detr_large
read_only: true
labels:
com.nodedc.product: mission-core
com.nodedc.stack: ai-polygon
com.nodedc.role: detector
com.nodedc.managed-by: ai-polygon-worker
networks:
default:
name: ndc-ai-polygon-models
labels:
com.nodedc.product: mission-core
com.nodedc.stack: ai-polygon
com.nodedc.role: inference
com.nodedc.managed-by: ai-polygon-worker
+6
View File
@@ -0,0 +1,6 @@
#!/bin/sh
set -eu
# Dedicated simulation channel. Existing Observatory port 18080 is untouched.
exec ssh -N -T -o BatchMode=yes -o ExitOnForwardFailure=yes \
-o ServerAliveInterval=5 -o ServerAliveCountMax=3 \
-R 127.0.0.1:18081:127.0.0.1:8000 mission-gpu
+97
View File
@@ -0,0 +1,97 @@
"""Private loopback transport over the operator's dedicated SSH reverse tunnel."""
import hashlib
import http.client
import json
import threading
import time
from pathlib import Path
from urllib.parse import urlsplit
PREFIX = "/api/v1/ai-polygon"
class CoreClient:
def __init__(self, origin: str, token_file: Path, instance: str):
url = urlsplit(origin)
if (
url.scheme != "http"
or url.hostname != "127.0.0.1"
or url.path
or url.query
or url.fragment
or url.username
or not url.port
):
raise ValueError("Core must use an explicit loopback SSH tunnel origin")
self.port = url.port
self.token = token_file.read_text().strip()
if not 32 <= len(self.token) <= 512:
raise ValueError("Invalid simulation credential")
self.instance = instance
self._connections = threading.local()
def request(self, path: str, body=None):
connection = getattr(self._connections, "connection", None)
if (
connection is not None
and time.monotonic() - getattr(self._connections, "last_used", 0) > 2
):
connection.close()
connection = None
if connection is None:
connection = http.client.HTTPConnection("127.0.0.1", self.port, timeout=10)
self._connections.connection = connection
try:
connection.request(
"GET" if body is None else "POST",
PREFIX + path,
body=None if body is None else json.dumps(body, allow_nan=False).encode(),
headers={
"Authorization": "Bearer " + self.token,
"Worker-Instance": self.instance,
"Content-Type": "application/json",
},
)
response = connection.getresponse()
raw = response.read(4 * 1024**2 + 1)
if response.status not in (200, 201) or len(raw) > 4 * 1024**2:
raise RuntimeError("Core rejected simulation operation: " + str(response.status))
self._connections.last_used = time.monotonic()
return json.loads(raw)
except Exception:
connection.close()
self._connections.connection = None
# Never retry an uncertain mutation automatically.
raise
def download(self, world: dict, target: Path):
if target.exists():
with target.open("rb") as stream:
if hashlib.file_digest(stream, "sha256").hexdigest() == world["sha256"]:
return
if world.get("storage", {}).get("kind") == "worker":
raise RuntimeError("The admitted Worker-local scene is missing or changed")
connection = http.client.HTTPConnection("127.0.0.1", self.port, timeout=30)
temporary = target.with_suffix(".part")
try:
connection.request("GET", PREFIX + "/worlds/" + world["world_id"] + "/source.ply")
response = connection.getresponse()
if (
response.status != 200
or int(response.getheader("Content-Length") or 0) != world["byte_length"]
):
raise RuntimeError("Scene transfer contract changed")
digest, size = hashlib.sha256(), 0
with temporary.open("wb") as stream:
while block := response.read(4 * 1024**2):
size += len(block)
if size > world["byte_length"]:
raise RuntimeError("Scene transfer exceeds admitted size")
digest.update(block)
stream.write(block)
if size != world["byte_length"] or digest.hexdigest() != world["sha256"]:
raise RuntimeError("Scene transfer identity changed")
temporary.replace(target)
finally:
connection.close()
+83
View File
@@ -0,0 +1,83 @@
"""Resident reference DDRNet adapter; run only inside the pinned module image.
Raw pinhole RGB is the only input. No device mask, scene labels or actor truth.
The container publishes this port on Windows loopback only and owns no weights.
"""
import hashlib
import importlib.util
import json
from http.server import BaseHTTPRequestHandler, HTTPServer
from pathlib import Path
import numpy as np
from PIL import Image
def checked(path, expected):
value = Path(path)
if hashlib.sha256(value.read_bytes()).hexdigest() != expected:
raise RuntimeError("Pinned DDRNet asset changed: " + value.name)
return value
def main():
checkpoint = checked(
"/assets/ddrnet-checkpoint.pth",
"b99c2838051bcd7b092fd3970aa62a77d5c0bbb809c9b9afb2ff4b0ebdaa4ee6",
)
runner = checked(
"/assets/ddrnet-goose-runner.py",
"b18ad60f277eea69a240a28f290611b94627fb9707faf1bb3e6e22102dad67c1",
)
spec = importlib.util.spec_from_file_location("polygon_pinned_goose", runner)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
model, _, _ = module.load_model("ddrnet", checkpoint)
tensor, _ = module.preprocess(Image.fromarray(np.zeros((600, 800, 3), np.uint8)))
module.infer(model, tensor)
class Handler(BaseHTTPRequestHandler):
def setup(self):
super().setup()
self.connection.settimeout(10)
def reply(self, status, body, kind="application/octet-stream"):
self.send_response(status)
self.send_header("Content-Type", kind)
self.send_header("Content-Length", str(len(body)))
self.end_headers()
self.wfile.write(body)
def do_GET(self):
if self.path != "/ready":
self.reply(404, b"")
return
self.reply(200, json.dumps({"model": "ddrnet-goose-pytorch-reference"}).encode())
def do_POST(self):
if self.path != "/infer" or self.headers.get("Content-Length") != "1440000":
self.reply(400, b"Expected 800x600 raw RGB uint8")
return
try:
raw = self.rfile.read(1440000)
if len(raw) != 1440000:
raise ValueError("Incomplete camera frame")
rgb = np.frombuffer(raw, np.uint8).reshape(600, 800, 3)
tensor, _ = module.preprocess(Image.fromarray(rgb))
mask, _ = module.infer(model, tensor)
if mask.shape != (512, 512) or np.any(mask < 0) or np.any(mask >= 64):
raise ValueError("DDRNet output contract changed")
self.reply(200, mask.astype(np.uint8).tobytes())
except (TimeoutError, ValueError, RuntimeError):
self.reply(500, b"DDRNet inference failed")
def log_message(self, *_):
pass
# External exposure is restricted by the Compose loopback publication.
HTTPServer(("0.0.0.0", 8010), Handler).serve_forever()
if __name__ == "__main__":
main()
+86
View File
@@ -0,0 +1,86 @@
"""Archive full-resolution chunks from an authorized public streamed-SOG scene."""
import argparse
import gzip
import hashlib
import json
import re
import subprocess
import sys
import urllib.request
from pathlib import Path
from urllib.parse import urljoin
parser = argparse.ArgumentParser()
parser.add_argument("--metadata-url", required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--author", required=True)
parser.add_argument("--license", required=True)
parser.add_argument("--source-url", required=True)
args = parser.parse_args()
if not args.metadata_url.startswith("https://") or not args.metadata_url.endswith("/lod-meta.json"):
parser.error("Use an observed public HTTPS lod-meta.json URL")
args.output.mkdir(parents=True, exist_ok=False)
with urllib.request.urlopen(args.metadata_url, timeout=30) as response:
raw = response.read(1024**2 + 1)
if len(raw) > 1024**2:
raise ValueError("LOD manifest too large")
if raw[:2] == b"\x1f\x8b":
raw = gzip.decompress(raw)
if len(raw) > 1024**2:
raise ValueError("LOD manifest too large")
meta = json.loads(raw)
if meta["version"] != 1 or not 1 <= meta["counts"][0] <= 20_000_000:
raise ValueError("Unsupported scene size or LOD version")
indices = set()
def visit(node):
if "0" in node.get("lods", {}):
indices.add(node["lods"]["0"]["file"])
for child in node.get("children", []):
visit(child)
visit(meta["tree"])
if not 1 <= len(indices) <= 100:
raise ValueError("Unexpected full-resolution chunk count")
(args.output / "lod-meta.json").write_bytes(raw)
for index in sorted(indices):
name = meta["filenames"][index]
if not re.fullmatch(r"[a-zA-Z0-9_-]+/meta\.json", name):
raise ValueError("Unexpected chunk reference")
command = [
sys.executable,
str(Path(__file__).with_name("fetch_sog.py")),
"--metadata-url",
urljoin(args.metadata_url, name),
"--output",
str(args.output / Path(name).parent),
"--author",
args.author,
"--license",
args.license,
"--source-url",
args.source_url,
]
subprocess.run(command, check=True)
manifest = {
"source_url": args.source_url,
"metadata_url": args.metadata_url,
"author": args.author,
"license": args.license,
"lod": 0,
"splat_count": meta["counts"][0],
"files": [
{
"path": p.relative_to(args.output).as_posix(),
"sha256": hashlib.sha256(p.read_bytes()).hexdigest(),
"byte_length": p.stat().st_size,
}
for p in sorted(args.output.rglob("*"))
if p.is_file()
],
}
(args.output / "source-manifest.json").write_text(json.dumps(manifest, indent=2))
print(json.dumps({"count": meta["counts"][0], "chunks": len(indices)}))
+86
View File
@@ -0,0 +1,86 @@
"""Archive a licensed, public unbundled SOG; no cookies or account tokens.
Use only a metadata URL observed in an authorized scene. This downloads assets,
not collision/voxel geometry. The source provenance is carried into the manifest.
"""
import argparse
import gzip
import hashlib
import json
import re
import time
import urllib.request
from concurrent.futures import ThreadPoolExecutor
from datetime import UTC, datetime
from pathlib import Path
from urllib.parse import urljoin, urlsplit
parser = argparse.ArgumentParser()
parser.add_argument("--metadata-url", required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--author", required=True)
parser.add_argument("--license", required=True)
parser.add_argument("--source-url", required=True)
args = parser.parse_args()
url = urlsplit(args.metadata_url)
if url.scheme != "https" or url.username or not url.path.endswith("/meta.json"):
parser.error("Use a public HTTPS SOG meta.json URL")
args.output.mkdir(parents=True, exist_ok=False)
started = time.monotonic_ns()
def fetch(name, limit):
target = args.output / name
digest = hashlib.sha256()
length = 0
with (
urllib.request.urlopen(urljoin(args.metadata_url, name), timeout=30) as response,
target.with_suffix(target.suffix + ".part").open("xb") as stream,
):
while data := response.read(1024 * 1024):
length += len(data)
if length > limit:
raise ValueError("SOG asset exceeds bounded download size")
stream.write(data)
digest.update(data)
target.with_suffix(target.suffix + ".part").replace(target)
return {"path": name, "byte_length": length, "sha256": digest.hexdigest()}
meta = fetch("meta.json", 1024 * 1024)
raw = (args.output / "meta.json").read_bytes()
if raw[:2] == b"\x1f\x8b":
raw = gzip.decompress(raw)
if len(raw) > 1024 * 1024:
raise ValueError("SOG metadata exceeds bounded size")
(args.output / "meta.json").write_bytes(raw)
meta = {"path": "meta.json", "byte_length": len(raw), "sha256": hashlib.sha256(raw).hexdigest()}
metadata = json.loads(raw)
if metadata.get("version") != 2 or not 1 <= metadata["count"] <= 20_000_000:
raise ValueError("Unsupported SOG version or size")
files = sorted(
{
name
for value in metadata.values()
if isinstance(value, dict)
for name in value.get("files", [])
}
)
if not 1 <= len(files) <= 10 or any(not re.fullmatch(r"[A-Za-z0-9_-]+\.webp", n) for n in files):
raise ValueError("SOG contains unexpected asset references")
with ThreadPoolExecutor(max_workers=3) as pool:
rows = list(pool.map(lambda name: fetch(name, 100 * 1024**2), files))
report = {
"schema_version": "missioncore.ai-polygon-source-archive/v1",
"source_url": args.source_url,
"metadata_url": args.metadata_url,
"author": args.author,
"license": args.license,
"captured_at": datetime.now(UTC).isoformat(),
"elapsed_ns": time.monotonic_ns() - started,
"splat_count": metadata["count"],
"files": [meta, *rows],
}
(args.output / "source-manifest.json").write_text(json.dumps(report, indent=2))
print(json.dumps({"count": report["splat_count"], "bytes": sum(r["byte_length"] for r in rows)}))
+102
View File
@@ -0,0 +1,102 @@
"""Atomic local IPC snapshots; no network access in the simulation process."""
import json
import os
import sqlite3
import time
from pathlib import Path
class StateChannel:
"""Latest-value local IPC using SQLite WAL, available in both Python hosts.
Readers never hold Windows rename/delete locks on a telemetry snapshot.
The durable camera/motion journals remain separate evidence artifacts.
Each process owns its connection; only small control/telemetry JSON enters it.
"""
def __init__(self, directory: Path):
self.connection = sqlite3.connect(directory / "channel.sqlite3", timeout=0.1)
self.connection.execute("PRAGMA journal_mode=WAL")
self.connection.execute("PRAGMA synchronous=NORMAL")
self.connection.execute(
"CREATE TABLE IF NOT EXISTS state (key TEXT PRIMARY KEY, value TEXT NOT NULL)"
)
self.connection.commit()
def write(self, key, value):
encoded = json.dumps(value, allow_nan=False)
with self.connection:
self.connection.execute(
"INSERT INTO state(key,value) VALUES(?,?) "
"ON CONFLICT(key) DO UPDATE SET value=excluded.value",
(key, encoded),
)
def read(self, key, default=None):
row = self.connection.execute("SELECT value FROM state WHERE key=?", (key,)).fetchone()
return default if row is None else json.loads(row[0])
def close(self):
self.connection.close()
def sharing_retry(operation):
# A concurrent Windows reader briefly denies delete/replace sharing. Retry
# only this local file operation, bounded to 14ms; permanent errors surface.
for attempt in range(8):
try:
return operation()
except PermissionError:
if attempt == 7:
raise
time.sleep(0.002)
def write_json(path: Path, value):
temporary = path.with_name(path.name + f".{os.getpid()}.tmp")
temporary.write_text(json.dumps(value, allow_nan=False), encoding="utf-8")
sharing_retry(lambda: os.replace(temporary, path))
def read_json(path: Path, default=None):
try:
return json.loads(sharing_retry(lambda: read_shared_text(path)))
except FileNotFoundError:
return default
def read_shared_text(path: Path):
if os.name != "nt":
return path.read_text(encoding="utf-8")
# Python's normal Windows open does not grant FILE_SHARE_DELETE. An atomic
# writer must remain able to replace a snapshot while a reader holds it.
import ctypes
import msvcrt
from ctypes import wintypes
kernel = ctypes.WinDLL("kernel32", use_last_error=True)
kernel.CreateFileW.argtypes = [
wintypes.LPCWSTR,
wintypes.DWORD,
wintypes.DWORD,
ctypes.c_void_p,
wintypes.DWORD,
wintypes.DWORD,
wintypes.HANDLE,
]
kernel.CreateFileW.restype = wintypes.HANDLE
handle = kernel.CreateFileW(str(path), 0x80000000, 7, None, 3, 0x80, None)
if handle == wintypes.HANDLE(-1).value:
code = ctypes.get_last_error()
if code in (2, 3):
raise FileNotFoundError(str(path))
raise ctypes.WinError(code)
try:
descriptor = msvcrt.open_osfhandle(handle, os.O_RDONLY)
except BaseException:
kernel.CloseHandle.argtypes = [wintypes.HANDLE]
kernel.CloseHandle(handle)
raise
with os.fdopen(descriptor, "r", encoding="utf-8") as stream:
return stream.read()
+155
View File
@@ -0,0 +1,155 @@
"""Run-owned additive Docker stack. Caller must hold Core's GPU reservation."""
import hashlib
import http.client
import json
import subprocess
import sys
import time
from pathlib import Path
ROOT = Path(__file__).resolve().parent
sys.path.insert(0, str(ROOT.parents[1] / "src"))
NAVIGATION_NAME = "ndc-mission-core-ai-module-simulation-cmu-navigation"
NAMES = (
"ndc-ai-polygon-ddrnet",
"ndc-ai-polygon-segformer",
"ndc-ai-polygon-rfdetr",
NAVIGATION_NAME,
)
def sha256(path):
with Path(path).open("rb") as stream:
return hashlib.file_digest(stream, "sha256").hexdigest()
def docker(*args, check=True):
return subprocess.run(
["docker", *args], check=check, capture_output=True, text=True, timeout=120
)
class ModelStack:
def __init__(self):
self.profile_path = ROOT / "models.worker-006.json"
self.profile = json.loads(self.profile_path.read_text())
self.ids = []
def preflight(self):
from k1link.simulation.ai_polygon.composition import compose
compose(ROOT) # Portable graph admission happens before any Isaac boot.
for model in [*self.profile["models"], self.profile["navigation"]]:
for key in ("checkpoint", "runner"):
if key in model and sha256(model[key]) != model[key + "_sha256"]:
raise RuntimeError("Simulation model identity changed: " + model["id"])
actual = docker(
"image", "inspect", model["image"], "--format", "{{.Id}}"
).stdout.strip()
if actual != model["image"]:
raise RuntimeError("Simulation model image is unavailable")
if sha256(self.profile["labels"]) != self.profile["labels_sha256"]:
raise RuntimeError("GOOSE label table changed")
def start(self, cancelled=None, on_acquired=None, selection=None):
from k1link.simulation.ai_polygon.composition import compose
graph = compose(ROOT, selection)
services = {
"simulation-ddrnet-goose": "ddrnet",
"simulation-segformer-ade": "segformer",
"simulation-rf-detr": "detector",
}
module_ids = [node.module.module_id for node in graph.nodes]
installed = {*services, "simulation-cmu-navigation", "simulation-waypoint-mission"}
if set(module_ids) - installed:
raise ValueError("Uninstalled simulation provider")
self.preflight()
for name in NAMES:
if docker("container", "inspect", name, check=False).returncode == 0:
raise RuntimeError(
"An existing simulation container requires reconciliation: " + name
)
if cancelled and cancelled():
raise InterruptedError("Model startup cancelled")
try:
selected = [services[module] for module in module_ids if module in services]
docker("compose", "-f", str(ROOT / "compose.models.yaml"), "up", "-d", *selected)
if "simulation-cmu-navigation" in module_ids:
docker(
"run",
"-d",
"--name",
NAVIGATION_NAME,
"--restart",
"no",
"--label",
"com.nodedc.product=mission-core",
"--label",
"com.nodedc.stack=ai-polygon",
"--label",
"com.nodedc.role=ai-module",
"--label",
"com.nodedc.managed-by=ai-polygon-worker",
"--label",
"com.nodedc.composition-sha256=" + graph.sha256,
"--cpus",
"3",
"--memory",
"2g",
"-p",
"127.0.0.1:18093:8010",
"--mount",
f"type=bind,source={ROOT},target=/adapter,readonly",
self.profile["navigation"]["image"],
)
finally:
# Capture immutable IDs even after a partial Compose start.
for name in NAMES:
result = docker("container", "inspect", name, check=False)
if result.returncode == 0:
row = json.loads(result.stdout)[0]
if row["Config"]["Labels"].get("com.nodedc.stack") == "ai-polygon":
self.ids.append(row["Id"])
if on_acquired:
on_acquired(list(self.ids))
deadline = time.monotonic() + 120
while time.monotonic() < deadline:
if cancelled and cancelled():
raise InterruptedError("Model startup cancelled")
ready = True
for port, path in (
(18091, "/ready"),
(18092, "/v2/models/rf_detr_large/versions/1/ready"),
(18093, "/ready"),
):
connection = http.client.HTTPConnection("127.0.0.1", port, timeout=2)
try:
connection.request("GET", path)
response = connection.getresponse()
response.read(65536)
ready &= response.status == 200
except OSError:
ready = False
finally:
connection.close()
if ready:
return
time.sleep(0.5)
raise TimeoutError("Simulation models did not become ready")
def stop(self):
failures = []
for identity in self.ids:
result = docker("container", "rm", "-f", identity, check=False)
if (
result.returncode
and docker("container", "inspect", identity, check=False).returncode == 0
):
failures.append(identity)
if failures:
raise RuntimeError("Simulation GPU resources were not released")
self.ids.clear()
# Only our now-unused network; Docker refuses removal if another endpoint uses it.
docker("network", "rm", "ndc-ai-polygon-models", check=False)
@@ -0,0 +1,52 @@
{
"schema_version": "missioncore.ai-polygon-model-profile/v1",
"profile_id": "simulation-pinhole-composable-surfaces-cmu-v2",
"camera": {
"width": 800,
"height": 600,
"color": "RGB",
"projection": "pinhole"
},
"authority": "virtual-only",
"models": [
{
"id": "ddrnet-goose-pytorch-reference",
"image": "sha256:a3b7d22f5d3bfdf2d84444b936c8b01abf8243be652387d7e2024ba7bda587f5",
"checkpoint": "D:/NDC_MISSIONCORE/runtime/assets/observatory-portable/lab-v1-static-files-v1/ddrnet-checkpoint.pth",
"checkpoint_sha256": "b99c2838051bcd7b092fd3970aa62a77d5c0bbb809c9b9afb2ff4b0ebdaa4ee6",
"preprocess": "pinned GOOSE 600-square crop, nearest 512, RGB /255",
"runner": "D:/NDC_MISSIONCORE/runtime/assets/observatory-portable/lab-v1-static-files-v1/ddrnet-goose-runner.py",
"runner_sha256": "b18ad60f277eea69a240a28f290611b94627fb9707faf1bb3e6e22102dad67c1"
},
{
"id": "rf_detr_large",
"image": "sha256:58df7489c3f2276f9591d500a012dee03e23d35543ce3c390b4c001e6bf90794",
"checkpoint": "D:/NDC_MISSIONCORE/runtime/experiments/m48t-fixed-detector-20260825T095425Z/triton-models/rf_detr_large/1/model.plan",
"checkpoint_sha256": "986399ce706b7380472cf5e473232249fed6e628971d8007f6609e83128d46b8",
"preprocess": "full pinhole frame, resize 704x704, ImageNet normalization; no KB4 mask"
},
{
"id": "segformer-b2-ade150",
"image": "sha256:f37681beb9feecee991e71a60a68259f15b56b168c939db3c3cb259c322e9536",
"checkpoint": "D:/NDC_MISSIONCORE/runtime/simulation/assets/segformer-b2-ade/de01bae28967510f9ddd496c60a969357195400c/pytorch_model.bin",
"checkpoint_sha256": "187ca07bea003a5717c63d04ea90b07f33cd033c0ebf44b4b89fce5070d6c8f3",
"revision": "de01bae28967510f9ddd496c60a969357195400c",
"preprocess": "RGB centre 600-square crop; pinned processor bilinear 512, ImageNet normalization; logits bilinear 512; candidate confidence >=0.55",
"config_sha256": "ee7400840fdb1e5045f0b2eba78bf053df8e33a309c4acec31705a48c8cc5c00",
"processor_sha256": "8039d1d210abaa7117ad78e58cdfd6141a2ec72c03dae891b3cd76737e422c6c"
}
],
"labels": "D:/NDC_MISSIONCORE/runtime/assets/observatory-portable/lab-v1-static-files-v1/ddrnet-goose-mapping.csv",
"labels_sha256": "88ae319ba5a3877dd3ae0773f693a6a5fdc283934140de9dfaff029108aefd7f",
"navigation": {
"id": "cmu-terrain-navigation",
"image": "sha256:698228d0d378b166ceaf299aa86ab95b5e74179837532f2378c036c7f7621dcc",
"upstream_commit": "158e67b31b644ed1e8b06eb1d7f70e183cc62591"
},
"default_modules": [
"simulation-segformer-ade",
"simulation-rf-detr",
"simulation-cmu-navigation",
"simulation-waypoint-mission"
]
}
+36
View File
@@ -0,0 +1,36 @@
"""Physics-clock command envelope; perception remains asynchronous.
Limit wheel-surface acceleration without delaying a safety reduction. Scaling
both sides together preserves the planner's requested curvature. Stops bypass
the ramp, as required by the already qualified collision/braking envelope.
This is a simulation actuator, not a VESC controller or a motor calibration.
"""
CONTROL_PROFILE = {
"clock": "physics-pre-step",
"rate_hz": 60,
"wheel_surface_acceleration_mps2": 0.2,
"safety_reductions": "immediate",
"authority": "simulation-only",
}
class DriveEnvelope:
def __init__(self, track_width=0.9, acceleration=0.2):
self.half_track = track_width / 2
self.acceleration = acceleration
self.wheels = (0.0, 0.0)
def step(self, velocity, yaw_rate, dt, *, stop=False):
requested = (velocity - yaw_rate * self.half_track, velocity + yaw_rate * self.half_track)
if stop or max(map(abs, requested)) < 1e-8:
self.wheels = (0.0, 0.0)
return 0.0, 0.0
scale = 1.0
for old, new in zip(self.wheels, requested, strict=True):
# Reversal starts from zero; never retain motion in the old direction.
prior = abs(old) if old * new >= 0 else 0.0
if abs(new) > prior:
scale = min(scale, (prior + self.acceleration * max(0.0, dt)) / abs(new))
self.wheels = tuple(value * scale for value in requested)
return velocity * scale, yaw_rate * scale
@@ -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"]
@@ -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"
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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>
@@ -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
@@ -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)
@@ -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;
}
@@ -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))
+294
View File
@@ -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()
@@ -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)}))
@@ -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()
@@ -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()
@@ -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()
+121
View File
@@ -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()
+449
View File
@@ -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()
@@ -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);}
@@ -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)
+433
View File
@@ -0,0 +1,433 @@
"""Installed provider adapters executed in shared composition dependency order."""
import http.client
import json
import math
import time
from concurrent.futures import ThreadPoolExecutor
from pathlib import Path
import numpy as np
from k1link.simulation.ai_polygon.composition import compose
from k1link.simulation.ai_polygon.contracts import Decision
from k1link.simulation.ai_polygon.inference import ModelInference
from k1link.simulation.ai_polygon.mission_policy import WaypointMission
def project(points, calibration):
rotation = np.asarray(calibration["rotation"], dtype=np.float64).reshape(3, 3)
local = (points - np.asarray(calibration["origin"])) @ rotation
front = local[:, 0] > 0.1
depth = np.maximum(local[:, 0], 0.1)
fx, fy, cx, cy = calibration["intrinsics"]
u = cx - fx * local[:, 1] / depth
v = cy - fy * local[:, 2] / depth
visible = front & (u >= 100) & (u < 700) & (v >= 0) & (v < 600)
return local, u, v, visible
def visual_goal(surface, points, pose, calibration, prior=None, target=None, excluded=()):
"""Project semantic surface candidates into range; CMU owns path search."""
local, u, v, visible = project(points, calibration)
semantic = np.zeros(len(points), dtype=bool)
semantic[visible] = surface[
((v[visible] * 512 / 600).astype(int)), (((u[visible] - 100) * 512 / 600).astype(int))
]
ground_z = pose[2] - calibration.get("body_contact_height_m", 0.27)
distance = np.linalg.norm(points[:, :2] - np.asarray(pose[:2]), axis=1)
slope = np.abs(points[:, 2] - ground_z) / np.maximum(distance, 0.1)
good = (
visible
& semantic
& (distance >= (0.4 if target is not None else 1.2))
& (distance <= 3.5)
& (slope <= np.tan(np.radians(25)))
& (local[:, 2] < 0)
)
candidates = points[good]
# A pixel at the centre is insufficient for a metre-wide chassis. Require
# visual surface support on both sides, at the same candidate ground level.
if len(candidates):
delta = candidates[:, :2] - np.asarray(pose[:2])
perpendicular = np.column_stack((-delta[:, 1], delta[:, 0]))
perpendicular /= np.maximum(np.linalg.norm(perpendicular, axis=1)[:, None], 0.1)
supported = np.ones(len(candidates), dtype=bool)
for side in (-0.5, 0.5):
edge = candidates.copy()
edge[:, :2] += perpendicular * side
_, eu, ev, in_view = project(edge, calibration)
ok = np.zeros(len(edge), dtype=bool)
ok[in_view] = surface[
(ev[in_view] * 512 / 600).astype(int), ((eu[in_view] - 100) * 512 / 600).astype(int)
]
supported &= ok
for failed in excluded:
supported &= np.linalg.norm(candidates[:, :2] - np.asarray(failed[:2]), axis=1) > 0.6
good[np.flatnonzero(good)[~supported]] = False
candidates = points[good]
if len(candidates) < 8:
return None
if target is not None:
target = np.asarray(target)
if prior is not None and np.linalg.norm(np.asarray(prior[:2]) - target) < 1e-4:
direction = target - pose[:2]
remaining = np.linalg.norm(direction)
lateral = np.array([-direction[1], direction[0]]) / max(remaining, 0.1)
footprint = np.tile(prior, (3, 1))
footprint[:, :2] += np.array([-0.5, 0, 0.5])[:, None] * lateral
_, pu, pv, in_view = project(footprint, calibration)
# The measured camera projection defines its blind strip; a fixed
# 0.8 m cutoff forgot goals that disappeared around 0.95 m. Retain
# only the exact, previously observed task goal. Any still-visible
# non-drivable part invalidates it; live CMU range/collision checks
# remain mandatory before every motion command.
if 0.35 < remaining <= 3.5 and not in_view.all():
if surface[
(pv[in_view] * 512 / 600).astype(int),
((pu[in_view] - 100) * 512 / 600).astype(int),
].all():
return prior
return None
near_target = np.linalg.norm(candidates[:, :2] - target, axis=1) <= 0.25
if near_target.sum() >= 3:
# Once the requested waypoint has observed semantic/range support,
# keep its XY identity. A moving pixel median can slide laterally
# past the arrival radius, leaving CMU chasing successive forward
# goals after the actual task waypoint is already behind the rover.
requested = np.array([*target.tolist(), float(np.median(candidates[near_target, 2]))])
direction = target - pose[:2]
lateral = np.array([-direction[1], direction[0]]) / max(
np.linalg.norm(direction), 0.1
)
footprint = np.tile(requested, (3, 1))
footprint[:, :2] += np.array([-0.5, 0, 0.5])[:, None] * lateral
_, tu, tv, in_view = project(footprint, calibration)
if in_view.all() and surface[
(tv * 512 / 600).astype(int), ((tu - 100) * 512 / 600).astype(int)
].all():
return requested.tolist()
if prior is not None and target is not None:
remaining = np.linalg.norm(np.asarray(prior[:2]) - pose[:2])
# Finish an already observed close waypoint after it enters the camera's
# under-body blind strip. Current visual support is still required above;
# CMU's causal terrain memory and live collision monitor retain authority.
if 0.35 < remaining <= 0.8:
return prior
# Retain a still-observed waypoint until reached; never keep a stale visual
# goal after the supporting surface disappears or changes classification.
if (
prior is not None
and np.linalg.norm(np.asarray(prior[:2]) - pose[:2]) > 0.8
and np.linalg.norm(candidates[:, :2] - np.asarray(prior[:2]), axis=1).min() < 0.35
):
return prior
good_local = local[good]
score = np.abs(distance[good] - 2.3) + 2.0 * np.abs(
np.arctan2(good_local[:, 1], good_local[:, 0])
)
if target is not None:
remaining = np.linalg.norm(target - pose[:2])
target_distance = np.linalg.norm(candidates[:, :2] - target, axis=1)
progress = target_distance < remaining - 0.1
if not progress.any():
return None
score = target_distance + 0.2 * score
score[~progress] = np.inf
center = candidates[int(np.argmin(score))]
neighbors = candidates[np.linalg.norm(candidates[:, :2] - center[:2], axis=1) < 0.3]
return np.median(neighbors, axis=0).tolist()
def recovery_goal(points, pose, calibration, prior=None):
"""A short straight retreat requires fresh support across its whole width.
This is admission of a task goal, not a replacement for CMU. Its obstacle
map, path selector and swept-body monitor still reject the actual command.
Missing returns, ledges, steep or discontinuous ground forbid retreat.
"""
points = np.asarray(points)
qx, qy, qz, qw = pose[3:]
yaw = math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz))
axes = np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
local = (points[:, :2] - np.asarray(pose[:2])) @ axes
if prior is None:
distance = 0.65
else:
offset = (np.asarray(prior[:2]) - np.asarray(pose[:2])) @ axes
distance = -float(offset[0])
if not 0 < distance <= 0.8 or abs(offset[1]) > 0.1:
return None
ground = pose[2] - calibration["body_contact_height_m"]
heights = []
# 15 cm lateral reserve covers the small heading correction allowed during
# reverse. Never extrapolate through an unobserved cell behind either wheel.
for x in np.arange(-0.6 - distance, -0.5, 0.15):
for y in np.arange(-0.6, 0.61, 0.15):
near = np.linalg.norm(local - [x, y], axis=1) <= 0.18
if near.sum() < 3:
return None
z = points[near, 2]
if np.ptp(z) > 0.1 or abs(float(np.median(z)) - ground) > 0.1 + abs(x) * math.tan(
math.radians(25)
):
return None
heights.append([x, y, float(np.median(z))])
samples = np.asarray(heights)
design = np.column_stack((samples[:, :2], np.ones(len(samples))))
plane = np.linalg.lstsq(design, samples[:, 2], rcond=None)[0]
if np.linalg.norm(plane[:2]) > math.tan(math.radians(25)):
return None
if np.max(np.abs(samples[:, 2] - design @ plane)) > 0.05:
return None
if prior is not None:
return prior
xy = np.asarray(pose[:2]) + axes @ [-distance, 0]
return [*xy.tolist(), float(plane[2] - distance * plane[0])]
class NavigationClient:
def __init__(self):
self.connection = http.client.HTTPConnection("127.0.0.1", 18093, timeout=2)
def request(self, path, body=None):
self.connection.request(
"GET" if body is None else "POST",
path,
body=None if body is None else json.dumps(body, allow_nan=False),
headers={"Content-Type": "application/json"},
)
response = self.connection.getresponse()
raw = response.read(3_000_001)
if response.status != 200 or len(raw) > 3_000_000:
raise RuntimeError("Local CMU navigation is unavailable")
return json.loads(raw)
def close(self):
self.connection.close()
class ComposedInference:
def __init__(self, root: Path, run, directory: Path, mission=None):
profile = json.loads((root / "models.worker-006.json").read_text())
self.graph = compose(root, run["request"].get("composition"))
self.segmenter_id = next(
n.module.module_id for n in self.graph.nodes if n.module.group == "segmentation"
)
self.models = ModelInference(
"http://127.0.0.1:18092",
Path(profile["labels"]),
"http://127.0.0.1:18091",
segmenter_id=self.segmenter_id,
)
self.navigation = NavigationClient()
self.max_speed = run["world"]["settings"]["max_speed_mps"]
self.goal = None
self.mission = mission or WaypointMission(run["world"]["settings"].get("route_xy", []))
self.navigation_evidence = {}
self.pool = ThreadPoolExecutor(max_workers=2, thread_name_prefix="simulation-module")
self.directory = directory
self.providers = {
"simulation-ddrnet-goose": lambda inputs: self.models.surface(
inputs["source.camera.rgb"]
),
"simulation-segformer-ade": lambda inputs: self.models.surface(
inputs["source.camera.rgb"]
),
"simulation-rf-detr": lambda inputs: {
"detection.boxes": self.models.detect(inputs["source.camera.rgb"])
},
"simulation-cmu-navigation": self.navigate,
"simulation-waypoint-mission": self.plan_mission,
}
if {node.module.module_id for node in self.graph.nodes} - set(self.providers):
raise ValueError("Composition contains an uninstalled executor")
directory.mkdir(exist_ok=True, parents=True)
(directory.parent / "composition.json").write_text(
json.dumps(
{
"sha256": self.graph.sha256,
"graph": self.graph.as_dict(),
"modules": [node.module.identity_document() for node in self.graph.nodes],
},
indent=2,
),
encoding="utf-8",
)
def ready(self):
self.models.ready()
self.navigation.request("/ready")
def reset(self):
self.goal = None
self.mission.resume()
self.navigation.request("/reset", {})
deadline = time.monotonic() + 8
while time.monotonic() < deadline:
try:
self.navigation.request("/ready")
return
except (OSError, RuntimeError):
time.sleep(0.1)
raise RuntimeError("Navigation reset timed out")
def plan_mission(self, inputs):
self.goal, intent = self.mission.update(
inputs["source.pose"],
inputs["source.simulation-time"],
lambda target, prior, excluded: visual_goal(
inputs["segmentation.surface"],
inputs["source.lidar"],
inputs["source.pose"],
inputs["source.camera.calibration"],
prior,
target,
excluded,
),
lambda prior: recovery_goal(
inputs["source.lidar"],
inputs["source.pose"],
inputs["source.camera.calibration"],
prior,
),
)
return {"navigation.goal": self.goal, "navigation.intent": intent}
def navigate(self, inputs):
surface, boxes = inputs["segmentation.surface"], inputs["detection.boxes"]
points, pose = inputs["source.lidar"], inputs["source.pose"]
calibration = inputs["source.camera.calibration"]
intent = inputs["navigation.intent"]
fraction = float(surface[320:500, 100:412].mean())
reason, result = "uncertain", {"speed_mps": 0.0, "yaw_rate_rps": 0.0, "path": []}
if len(points) >= 50:
local, u, v, visible = project(points, calibration)
distance = np.linalg.norm(points[:, :2] - np.asarray(pose[:2]), axis=1)
danger = False
for x1, y1, x2, y2 in boxes:
covered = (
visible & (u >= 800 * x1) & (u <= 800 * x2) & (v >= 600 * y1) & (v <= 600 * y2)
)
if np.any(
covered & (distance < 1.5) & (local[:, 0] > 0) & (np.abs(local[:, 1]) < 0.8)
):
danger = True
if intent["state"] in {"stuck", "goal-reached", "unstable"}:
reason = intent["state"]
elif danger:
reason = "obstacle"
elif self.goal is None:
reason = "no-road"
else:
result = self.navigation.request(
"/plan",
{
"points": points.tolist(),
"pose": pose,
"body_contact_height_m": calibration["body_contact_height_m"],
"goal": self.goal,
"max_speed_mps": min(self.max_speed, 0.1)
if intent["state"] == "reversing"
else self.max_speed,
"allow_reverse": intent["state"] == "reversing",
},
)
reason = {
"path": "road",
"blocked": "obstacle",
"waiting-for-plan": "waiting",
}.get(result["status"], "uncertain")
if (
reason == "road"
and abs(result["speed_mps"]) + abs(result["yaw_rate_rps"]) < 1e-5
):
reason = "waiting"
elif reason == "road" and intent["state"] in {"replanning", "reversing"}:
reason = "replanning"
self.navigation_evidence = {
"mission": intent,
"navigation": {k: v for k, v in result.items() if k != "path"},
}
decision = Decision(
speed_mps=result["speed_mps"],
yaw_rate_rps=result["yaw_rate_rps"],
reason=intent["state"]
if intent["state"] in {"stuck", "goal-reached", "unstable"}
else reason,
road_fraction=fraction,
obstacle_count=len(boxes),
)
return {"motion.command": decision.model_dump(), "motion.path": result["path"]}
def infer_observation(self, rgb, observation, frame_id):
values = {
"source.camera.rgb": rgb,
"source.lidar": observation["points"],
"source.pose": observation["pose"],
"source.camera.calibration": observation["calibration"],
"source.simulation-time": observation["simulation_time_ns"] / 1e9,
}
timings = {}
pending = list(self.graph.nodes)
def execute(node, inputs):
started = time.monotonic()
output = self.providers[node.module.module_id](inputs)
if set(output) != set(node.module.provides):
raise ValueError("Provider output does not match its composition contract")
return output, (time.monotonic() - started) * 1000
while pending:
ready = [node for node in pending if all(port in values for port, _ in node.inputs)]
if not ready:
raise ValueError("Unresolved composition input")
tasks = [
(
node,
self.pool.submit(
execute, node, {port: values[port] for port, _ in node.inputs}
),
)
for node in ready
]
for node, task in tasks:
output, elapsed = task.result()
values.update(output)
timings[node.module.module_id] = elapsed
pending.remove(node)
from PIL import Image
Image.fromarray(values["segmentation.labels"]).save(
self.directory / f"{frame_id:08d}.labels.png"
)
Image.fromarray(values["segmentation.surface"].astype(np.uint8) * 255).save(
self.directory / f"{frame_id:08d}.surface.png"
)
np.savez_compressed(
self.directory / f"{frame_id:08d}.range.npz",
points=observation["points"],
pose=observation["pose"],
origin=observation["calibration"]["origin"],
rotation=observation["calibration"]["rotation"],
intrinsics=observation["calibration"]["intrinsics"],
body_contact_height_m=observation["calibration"]["body_contact_height_m"],
range_origins=observation["calibration"].get("range_origins", []),
)
return (
values["motion.command"],
values["detection.boxes"],
{
"composition_sha256": self.graph.sha256,
"module_ms": timings,
"goal": self.goal,
"path": values["motion.path"],
"range_points": len(observation["points"]),
**self.navigation_evidence,
},
)
def close(self):
self.pool.shutdown(wait=True, cancel_futures=True)
self.models.close()
self.navigation.close()
+78
View File
@@ -0,0 +1,78 @@
"""Versioned Worker-only collision preparation; never uploads generated assets."""
import hashlib
import json
import subprocess
import time
from pathlib import Path
from local_state import write_json
from k1link.simulation.ai_polygon.terrain_contract import terrain_matches
def prepare_terrain(root: Path, episode: Path, world: dict, job=None, pulse=None) -> Path:
if world.get("storage", {}).get("kind") == "worker":
path = root / "assets/prepared-worlds" / world["sha256"] / "terrain.json"
manifest = json.loads(path.read_text(encoding="utf-8-sig"))
if (
manifest.get("generator") != "paired-source"
or manifest.get("collider_sha256") != world.get("collider_sha256")
or not terrain_matches(manifest, world)
):
raise ValueError("Paired collision asset does not match the admitted world")
return path # install_terrain hashes the actual collider before physics.
generator = Path(__file__).parent / "navigation/Prepare-Terrain.ps1"
generator_sha256 = hashlib.sha256(generator.read_bytes()).hexdigest()
def matching():
for path in (root / "assets/terrain-v1").glob(world["sha256"] + "-*/terrain.json"):
value = json.loads(path.read_text(encoding="utf-8-sig"))
if terrain_matches(value, world, generator_sha256):
return path
return None
found = matching()
if found:
return found
world_path = episode / "terrain-world.json"
write_json(world_path, world)
with (episode / "terrain-preparation.log").open("wb") as log:
process = subprocess.Popen(
[
"powershell.exe",
"-NoProfile",
"-NonInteractive",
"-ExecutionPolicy",
"Bypass",
"-File",
str(generator),
"-Root",
str(root),
"-WorldFile",
str(world_path),
],
stdout=log,
stderr=subprocess.STDOUT,
)
if job is not None:
job.assign(process)
deadline = time.monotonic() + 180
try:
while process.poll() is None:
if time.monotonic() > deadline:
raise TimeoutError("Terrain preparation timed out")
if pulse:
pulse()
time.sleep(1)
if process.returncode != 0:
raise RuntimeError("Terrain preparation failed; inspect the Worker episode log")
finally:
if process.poll() is None:
from worker import terminate_episode
terminate_episode(process)
found = matching()
if found is None:
raise RuntimeError("Terrain preparation did not produce a matching collider")
return found
@@ -0,0 +1,84 @@
"""Worker-only regression: observed retreat with the shipped body and sensors."""
import argparse
import hashlib
import json
import sys
import time
from datetime import UTC, datetime
from pathlib import Path
from isaacsim import SimulationApp
parser = argparse.ArgumentParser()
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
sys.path.insert(0, str(Path(__file__).resolve().parents[2] / "src"))
app = SimulationApp({"headless": True, "hide_ui": True})
try:
import numpy as np
import omni.timeline
import omni.usd
from isaacsim.core.experimental.prims import Articulation
from isaacsim.core.simulation_manager import SimulationManager
from navigation_client import recovery_goal
from pxr import Gf, UsdGeom, UsdPhysics
from rover_profile import PROFILE, DifferentialDrive, create_rover
from terrain import RangeSensor
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
floor = UsdGeom.Cube.Define(stage, "/World/Ground")
floor.CreateSizeAttr(1)
floor.AddTranslateOp().Set(Gf.Vec3d(0, 0, -0.1))
floor.AddScaleOp().Set(Gf.Vec3f(8, 8, 0.2))
UsdPhysics.CollisionAPI.Apply(floor.GetPrim())
create_rover(stage, [0, 0, 0.29], 0)
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
omni.timeline.get_timeline_interface().play()
for _ in range(30):
app.update()
robot = Articulation("/World/Rover")
sensor = RangeSensor()
drive = DifferentialDrive(robot)
rows = []
for tick in range(25):
p, q = (v.numpy()[0] for v in robot.get_world_poses())
transform = Gf.Matrix4d(1).SetRotate(Gf.Quatd(float(q[0]), Gf.Vec3d(*map(float, q[1:]))))
transform.SetTranslateOnly(Gf.Vec3d(*map(float, p)))
origins = [
transform.Transform(Gf.Vec3d(x, 0, 0.8 - PROFILE["body_contact_height_m"]))
for x in (PROFILE["camera_forward_m"], PROFILE["rear_range_forward_m"])
]
points = sensor.capture(origins[0], transform, origins[1])
pose = [*p.tolist(), *q[1:].tolist(), float(q[0])]
goal = recovery_goal(
points, pose, {"body_contact_height_m": PROFILE["body_contact_height_m"]}
)
rows.append({"seconds": tick / 5, "pose": pose, "returns": len(points), "goal": goal})
drive.command(-0.1 if goal is not None else 0)
SimulationManager.step(steps=12)
drive.command(0)
before = robot.get_world_poses()[0].numpy()[0].copy()
SimulationManager.step(steps=120)
end = robot.get_world_poses()[0].numpy()[0]
drift = float(np.linalg.norm(end - before))
passed = all(row["goal"] is not None for row in rows) and end[0] < -0.4 and drift < 0.03
report = {
"utc": datetime.now(UTC).isoformat(),
"monotonic": time.monotonic(),
"profile": PROFILE,
"passed": bool(passed),
"end": end.tolist(),
"braking_drift_m": drift,
"samples": rows,
}
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
args.output.with_suffix(".sha256").write_text(
hashlib.sha256(args.output.read_bytes()).hexdigest(), encoding="ascii"
)
print(json.dumps({k: v for k, v in report.items() if k not in {"samples", "profile"}}))
finally:
app.close()
+103
View File
@@ -0,0 +1,103 @@
"""Bounded Worker-only drive/braking test, independent of AI and Gaussian mesh."""
import argparse
import hashlib
import json
import math
import time
from datetime import UTC, datetime
from pathlib import Path
from isaacsim import SimulationApp
parser = argparse.ArgumentParser()
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
app = SimulationApp({"headless": True, "hide_ui": True})
try:
import omni.timeline
import omni.usd
from isaacsim.core.experimental.prims import Articulation
from isaacsim.core.simulation_manager import SimulationManager
from pxr import Gf, UsdGeom, UsdPhysics, UsdShade
from rover_profile import PROFILE, DifferentialDrive, create_rover
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
ground = UsdGeom.Cube.Define(stage, "/World/Ground")
ground.CreateSizeAttr(1)
ground.AddTranslateOp().Set(Gf.Vec3d(0, 0, -0.1))
ground.AddScaleOp().Set(Gf.Vec3f(20, 20, 0.2))
UsdPhysics.CollisionAPI.Apply(ground.GetPrim())
material = UsdShade.Material.Define(stage, "/World/Material")
physics = UsdPhysics.MaterialAPI.Apply(material.GetPrim())
physics.CreateStaticFrictionAttr(0.9)
physics.CreateDynamicFrictionAttr(0.8)
UsdShade.MaterialBindingAPI.Apply(ground.GetPrim()).Bind(
material, UsdShade.Tokens.weakerThanDescendants, "physics"
)
create_rover(stage, [0, 0, PROFILE["wheel_radius_m"] + 0.05], 0)
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
omni.timeline.get_timeline_interface().play()
for _ in range(20):
app.update()
robot = Articulation("/World/Rover")
indices = robot.get_dof_indices(PROFILE["wheel_names"])
drive = DifferentialDrive(robot)
def position():
# Tensor pose is authoritative, independent of USD/Fabric writeback.
return robot.get_world_poses()[0].numpy().tolist()[0]
start = position()
drive.command(0.3)
SimulationManager.step(steps=300)
moved = position()
drive.command(0)
SimulationManager.step(steps=120)
stopped = position()
turns = []
def heading():
w, x, y, z = robot.get_world_poses()[1].numpy()[0].tolist()
return math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
for direction in (1, -1):
before, angle = position(), heading()
drive.command(0, direction * 0.35)
SimulationManager.step(steps=180)
turn = math.atan2(math.sin(heading() - angle), math.cos(heading() - angle))
after = position()
drive.command(0)
SimulationManager.step(steps=120)
turns.append(
dict(
command_yaw_rps=direction * 0.35,
measured_yaw_radians=turn,
displacement_m=math.dist(before[:2], after[:2]),
brake_drift_m=math.dist(after, position()),
passed=direction * turn > 0.5 and math.dist(before[:2], after[:2]) < 0.15,
)
)
report = dict(
utc=datetime.now(UTC).isoformat(),
monotonic=time.monotonic(),
profile_sha256=hashlib.sha256(
Path(__file__).with_name("rover_profile.py").read_bytes()
).hexdigest(),
profile=PROFILE,
start=start,
after_5s=moved,
after_stop_2s=stopped,
turns=turns,
passed=moved[0] - start[0] > 1.0
and abs(stopped[0] - moved[0]) < 0.15
and all(turn["passed"] for turn in turns),
wheel_velocity_rps=robot.get_dof_velocities(dof_indices=indices).numpy().tolist(),
)
args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
print(json.dumps(report))
finally:
app.close()
+116
View File
@@ -0,0 +1,116 @@
"""Bounded synthetic camera/model probe. This is NOT navigation acceptance.
Run with Isaac's python.bat after reserving Worker through the Core job queue.
Outputs stay private in the chosen evidence directory.
"""
import argparse
import hashlib
import json
import sys
import time
from datetime import UTC, datetime
from pathlib import Path
parser = argparse.ArgumentParser()
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--models", action="store_true")
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=False)
sys.path.insert(0, str(Path(__file__).resolve().parents[2] / "src"))
report = {
"schema_version": "missioncore.ai-polygon-runtime-probe/v1",
"started_at": datetime.now(UTC).isoformat(),
"started_monotonic_ns": time.monotonic_ns(),
"source": "synthetic-cube-only",
"navigation_accepted": False,
"passed": False,
}
app = stack = inference = None
try:
from isaacsim import SimulationApp
app = SimulationApp({"headless": True, "multi_gpu": False, "width": 800, "height": 600})
import numpy as np
import omni.usd
from isaacsim.core.experimental.utils import app as app_utils
from isaacsim.core.simulation_manager import SimulationManager
from isaacsim.sensors.experimental.rtx import CameraSensor, RtxCamera
from PIL import Image
from pxr import Gf, UsdGeom, UsdLux
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1.0)
cube = UsdGeom.Cube.Define(stage, "/World/Cube")
cube.AddTranslateOp().Set(Gf.Vec3d(0, 3, 0.5))
cube.CreateDisplayColorAttr([(0.8, 0.15, 0.05)])
UsdLux.DomeLight.Define(stage, "/World/Light").CreateIntensityAttr(500)
camera = RtxCamera(
"/World/Camera",
tick_rate=10,
translations=np.array([0.0, 0.0, 0.5]),
orientations=np.array([1.0, 1.0, 0.0, 0.0]) / np.sqrt(2),
)
camera.camera.set_focal_lengths(24.0)
sensor = CameraSensor(camera, resolution=(600, 800), annotators=["rgb"])
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
app_utils.play()
app_utils.update_app(steps=12)
app_utils.pause()
baseline = SimulationManager.get_num_physics_steps()
for _ in range(10):
app.update()
assert SimulationManager.get_num_physics_steps() == baseline, "Render advanced physics"
raw, _ = sensor.get_data("rgb")
assert raw is not None, "RTX camera did not produce a frame"
rgb = np.ascontiguousarray(raw.numpy()[:, :, :3])
assert rgb.shape == (600, 800, 3) and rgb.dtype == np.uint8
assert float(rgb.std()) > 1, "Camera frame is empty/uniform"
image_path = args.output / "synthetic-camera.png"
Image.fromarray(rgb).save(image_path)
report.update(
frame_sha256=hashlib.sha256(image_path.read_bytes()).hexdigest(),
frame_shape=list(rgb.shape),
paused_physics_steps=baseline,
)
SimulationManager.step(steps=6)
assert SimulationManager.get_num_physics_steps() == baseline + 6, "Wrong lockstep increment"
report["lockstep_physics_steps"] = 6
if args.models:
from model_stack import ModelStack
from k1link.simulation.ai_polygon.inference import ModelInference
from k1link.simulation.ai_polygon.policy import RoadPolicy
stack = ModelStack()
stack.start()
inference = ModelInference(
"http://127.0.0.1:18092", Path(stack.profile["labels"]), "http://127.0.0.1:18091"
)
inference.ready()
rows = []
policy = RoadPolicy(0.3)
for _ in range(3):
started = time.monotonic_ns()
road, boxes = inference.infer(rgb)
rows.append(
{
"inference_ms": (time.monotonic_ns() - started) / 1e6,
"decision": policy.decide(road, boxes).model_dump(),
}
)
report["model_probe"] = rows
report["passed"] = True
except Exception as exc:
report["error"] = type(exc).__name__ + ": " + str(exc)
raise
finally:
if inference is not None:
inference.close()
if stack is not None:
stack.stop()
report["finished_at"] = datetime.now(UTC).isoformat()
(args.output / "report.json").write_text(json.dumps(report, indent=2))
if app is not None:
app.close()
@@ -0,0 +1,101 @@
"""Worker-only contact replay on a retained episode's exact collision asset.
This isolates physical support from perception/planning. It never supplies a
route or collision truth to inference and never writes to the original run.
"""
import argparse
import hashlib
import json
import math
import sys
import time
from datetime import UTC, datetime
from pathlib import Path
from isaacsim import SimulationApp
parser = argparse.ArgumentParser()
parser.add_argument("--run", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
parser.add_argument("--mode", choices=("straight", "replay"), default="straight")
parser.add_argument("--seconds", type=float, default=40)
args = parser.parse_args()
sys.path.insert(0, str(Path(__file__).resolve().parents[2] / "src"))
app = SimulationApp({"headless": True, "hide_ui": True})
try:
import omni.timeline
import omni.usd
from isaacsim.core.experimental.prims import Articulation
from isaacsim.core.simulation_manager import SimulationManager
from pxr import UsdGeom
from rover_profile import PROFILE, DifferentialDrive, create_rover
from terrain import install_terrain
from k1link.simulation.ai_polygon.mission_policy import inclination
run = json.loads(args.run.read_text())
settings = run["world"]["settings"]
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
height, terrain = install_terrain(stage, run["terrain_manifest"], run["world"])
create_rover(
stage,
[*settings["spawn_xy"], height],
settings["heading_degrees"],
terrain["initial_ground_normal"],
)
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
omni.timeline.get_timeline_interface().play()
for _ in range(30):
app.update()
robot = Articulation("/World/Rover")
drive = DifferentialDrive(robot)
playback = []
if args.mode == "replay":
playback = [
json.loads(x) for x in (args.run.parent / "motion.jsonl").read_text().splitlines()
]
trace, cursor, stopped = [], 0, False
started = time.monotonic()
for step in range(round(args.seconds * 60)):
p, q = robot.get_world_poses()
p, q = p.numpy()[0].tolist(), q.numpy()[0].tolist()
pose = p + q[1:] + q[:1]
tilt = inclination(pose)
stopped |= tilt >= PROFILE["stop_tilt_degrees"]
v, w = settings["max_speed_mps"], 0.0
if playback:
while (
cursor + 1 < len(playback)
and playback[cursor + 1]["simulation_time_ns"] / 1e9 <= step / 60
):
cursor += 1
v, w = playback[cursor]["applied_speed_mps"], playback[cursor]["applied_yaw_rate_rps"]
drive.command(0 if stopped else v, 0 if stopped else w)
if step % 10 == 0:
trace.append(dict(time_s=step / 60, pose=pose, tilt_degrees=tilt, stopped=stopped))
SimulationManager.step(steps=1)
report = dict(
schema_version="missioncore.scene-contact-qualification/v1",
utc=datetime.now(UTC).isoformat(),
monotonic=time.monotonic(),
elapsed_wall_s=time.monotonic() - started,
run_sha256=hashlib.sha256(args.run.read_bytes()).hexdigest(),
mode=args.mode,
wheel_collision=PROFILE["wheel_collision"],
seconds=args.seconds,
profile=PROFILE,
terrain=terrain,
trace=trace,
displacement_m=math.dist(trace[0]["pose"][:2], trace[-1]["pose"][:2]),
max_tilt_degrees=max(x["tilt_degrees"] for x in trace),
stopped=stopped,
)
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
print(json.dumps({k: v for k, v in report.items() if k not in ("trace", "terrain", "profile")}))
finally:
app.close()
@@ -0,0 +1,105 @@
"""Worker-only landmark regression through the actual collision preparer.
The analytic landmark is transformed like the USD scan. This detects the
SplatTransform PLY convention and glTF basis mistakes before a scene is used.
"""
import argparse
import hashlib
import json
import struct
import subprocess
import time
from datetime import UTC, datetime
from pathlib import Path
import numpy as np
from terrain import load_glb
parser = argparse.ArgumentParser()
parser.add_argument("--root", type=Path, required=True)
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=True)
names = [
"x",
"y",
"z",
"f_dc_0",
"f_dc_1",
"f_dc_2",
"opacity",
"scale_0",
"scale_1",
"scale_2",
"rot_0",
"rot_1",
"rot_2",
"rot_3",
]
header = (
"ply\nformat binary_little_endian 1.0\nelement vertex 1\n"
+ "".join("property float " + n + "\n" for n in names)
+ "end_header\n"
)
payload = header.encode() + struct.pack("<14f", 1, 2, 3, 0, 0, 0, 9, -2.3, -2.3, -2.3, 1, 0, 0, 0)
sha = hashlib.sha256(payload).hexdigest()
# A private fixture root uses the installed tool directory but never overwrites
# a downloaded world or an existing terrain artifact.
fixture = args.output / "fixture"
(fixture / "state/worlds").mkdir(parents=True, exist_ok=True)
(fixture / "state/worlds" / (sha + ".ply")).write_bytes(payload)
tools = fixture / "tools"
if not tools.exists():
subprocess.run(
["cmd", "/c", "mklink", "/J", str(tools), str(args.root / "tools")],
check=True,
capture_output=True,
)
world = {
"sha256": sha,
"settings": {
"spawn_xy": [-1, -3],
"ground_z": -2,
"rotation_degrees": [-90, 0, 180],
"meters_per_unit": 1,
},
}
world_file = fixture / "world.json"
world_file.write_text(json.dumps(world), encoding="utf-8")
command = [
"powershell",
"-NoProfile",
"-ExecutionPolicy",
"Bypass",
"-File",
str(Path(__file__).parent / "navigation/Prepare-Terrain.ps1"),
"-Root",
str(fixture),
"-WorldFile",
str(world_file),
]
with (args.output / "prepare.log").open("w") as log:
subprocess.run(command, stdout=log, stderr=subprocess.STDOUT, check=True, timeout=90)
manifest_path = next((fixture / "assets/terrain-v1").glob("*/terrain.json"))
manifest = json.loads(manifest_path.read_text(encoding="utf-8-sig"))
points, _ = load_glb(manifest["collider"])
# Floor-fill extends the fixture downward. The sphere's upper surface is
# unaffected; its horizontal centre and uppermost Z must agree with the visual.
centre_xy = (points[:, :2].min(axis=0) + points[:, :2].max(axis=0)) / 2
expected = np.array([-1, -3])
passed = bool(np.linalg.norm(centre_xy - expected) < 0.06 and -2 < points[:, 2].max() < -1.6)
report = {
"utc": datetime.now(UTC).isoformat(),
"monotonic": time.monotonic(),
"source_sha256": sha,
"generator_sha256": manifest["generator_sha256"],
"expected_xy": expected.tolist(),
"actual_xy": centre_xy.tolist(),
"top_z": float(points[:, 2].max()),
"passed": passed,
}
(args.output / "report.json").write_text(json.dumps(report, indent=2), encoding="utf-8")
print(json.dumps(report))
if not passed:
raise SystemExit(1)
@@ -0,0 +1,135 @@
"""Worker-only physical capability measurement on isolated metric lanes.
All lanes share the shipped chassis and tire materials; no AI or Gaussian
geometry. Results qualify this virtual profile only, never the real vehicles.
"""
import argparse
import hashlib
import json
import math
import time
from datetime import UTC, datetime
from pathlib import Path
from isaacsim import SimulationApp
parser = argparse.ArgumentParser()
parser.add_argument("--output", type=Path, required=True)
args = parser.parse_args()
app = SimulationApp({"headless": True, "hide_ui": True})
try:
import omni.timeline
import omni.usd
from isaacsim.core.experimental.prims import Articulation
from isaacsim.core.simulation_manager import SimulationManager
from pxr import Gf, UsdGeom, UsdPhysics, UsdShade
from rover_profile import PROFILE, DifferentialDrive, create_rover
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
material = UsdShade.Material.Define(stage, "/World/Materials/Terrain")
physics = UsdPhysics.MaterialAPI.Apply(material.GetPrim())
physics.CreateStaticFrictionAttr(0.9)
physics.CreateDynamicFrictionAttr(0.8)
physics.CreateRestitutionAttr(0)
def surface(prim):
UsdPhysics.CollisionAPI.Apply(prim)
UsdShade.MaterialBindingAPI.Apply(prim).Bind(
material, UsdShade.Tokens.weakerThanDescendants, "physics"
)
def cube(path, center, scale):
shape = UsdGeom.Cube.Define(stage, path)
shape.CreateSizeAttr(1)
shape.AddTranslateOp().Set(Gf.Vec3d(*center))
shape.AddScaleOp().Set(Gf.Vec3f(*scale))
surface(shape.GetPrim())
cases = [("flat", 0)] + [("step", h) for h in (0.05, 0.1, 0.12, 0.14, 0.15, 0.18, 0.2, 0.25)]
cases += [("slope", a) for a in (10, 15, 20, 25)]
for i, (kind, value) in enumerate(cases):
y = i * 5.0
path = f"/World/Lane{i}"
cube(path + "/Ground", (3, y, -0.1), (12, 3, 0.2))
if kind == "step":
cube(path + "/Step", (4.5, y, value / 2), (6, 3, value))
elif kind == "slope":
# A continuous supported ramp; x=1.5 is the toe, no hidden step.
h = 6 * math.tan(math.radians(value))
ramp = UsdGeom.Mesh.Define(stage, path + "/Ramp")
ramp.CreatePointsAttr(
[(1.5, y - 1.5, 0), (1.5, y + 1.5, 0), (7.5, y - 1.5, h), (7.5, y + 1.5, h)]
)
ramp.CreateFaceVertexCountsAttr([3, 3])
ramp.CreateFaceVertexIndicesAttr([0, 2, 1, 1, 2, 3])
ramp.CreateSubdivisionSchemeAttr(UsdGeom.Tokens.none)
surface(ramp.GetPrim())
create_rover(stage, [0, y, PROFILE["wheel_radius_m"] + 0.04], 0, root_path=path + "/Rover")
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
omni.timeline.get_timeline_interface().play()
for _ in range(30):
app.update()
robots = [Articulation(f"/World/Lane{i}/Rover") for i in range(len(cases))]
indices = [r.get_dof_indices(PROFILE["wheel_names"]) for r in robots]
drives = [DifferentialDrive(r) for r in robots]
traces = [[] for _ in cases]
starts = [r.get_world_poses()[0].numpy()[0].tolist() for r in robots]
started = time.monotonic()
for drive in drives:
drive.command(0.3)
for tick in range(180):
SimulationManager.step(steps=10)
for i, robot in enumerate(robots):
pos, quat = robot.get_world_poses()
p, q = pos.numpy()[0], quat.numpy()[0]
tilt = math.degrees(
math.acos(max(-1, min(1, 1 - 2 * (float(q[1]) ** 2 + float(q[2]) ** 2))))
)
traces[i].append(dict(time_s=(tick + 1) / 6, xyz=p.tolist(), tilt_degrees=tilt))
if p[0] >= 5.5 or tilt > 35:
drives[i].command(0)
stops = [r.get_world_poses()[0].numpy()[0].tolist() for r in robots]
for drive in drives:
drive.command(0)
SimulationManager.step(steps=120)
rows = []
for i, (kind, value) in enumerate(cases):
end = robots[i].get_world_poses()[0].numpy()[0].tolist()
tilt = max(t["tilt_degrees"] for t in traces[i])
rows.append(
dict(
kind=kind,
value=value,
start=starts[i],
end=end,
max_tilt_degrees=tilt,
braking_drift_m=math.dist(stops[i], end),
wheel_velocity_rps=robots[i]
.get_dof_velocities(dof_indices=indices[i])
.numpy()
.tolist(),
reached=end[0] >= 5.4,
upright=tilt < 35,
passed=end[0] >= 5.4 and tilt < 35 and math.dist(stops[i], end) < 0.1,
trace=traces[i],
)
)
report = dict(
schema_version="missioncore.virtual-rover-capability/v1",
utc=datetime.now(UTC).isoformat(),
monotonic=time.monotonic(),
elapsed_wall_s=time.monotonic() - started,
profile=PROFILE,
profile_sha256=hashlib.sha256(
Path(__file__).with_name("rover_profile.py").read_bytes()
).hexdigest(),
cases=rows,
)
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
print(json.dumps({"cases": [{k: v for k, v in row.items() if k != "trace"} for row in rows]}))
finally:
app.close()
+154
View File
@@ -0,0 +1,154 @@
"""Worker-local, bounded asynchronous inference. No operator-network dependencies."""
import hashlib
import json
import threading
import time
import traceback
from pathlib import Path
class LatestInference:
def __init__(self, factory, policy, directory: Path, clock=time.monotonic):
self.factory, self.policy, self.directory, self.clock = factory, policy, directory, clock
self.condition = threading.Condition()
self.enabled = self.closed = self.ready = False
self.ever_ready = False
self.epoch = self.count = self.dropped = 0
self.pending = self.result = None
self.error = None
self.thread = threading.Thread(target=self._run, name="polygon-inference", daemon=True)
def start(self):
self.thread.start()
def enable(self, enabled):
with self.condition:
if enabled != self.enabled:
self.enabled = enabled
self.epoch += 1
self.pending = self.result = None
self.condition.notify_all()
def submit(self, rgb, frame_id, captured_at, simulation_time_ns, observation=None):
with self.condition:
if not self.enabled or self.closed:
return
if self.pending is not None:
self.dropped += 1
self.pending = (rgb, frame_id, captured_at, simulation_time_ns, self.epoch, observation)
self.condition.notify_all()
def command(self, now, frame_deadline=0.5, command_deadline=0.5):
with self.condition:
result = self.result
if not self.enabled:
return 0.0, 0.0, "paused", result
if self.error:
return 0.0, 0.0, "inference-error", result
if result is None or now - result["captured_at"] > frame_deadline:
return 0.0, 0.0, "stale-camera", result
if now - result["completed_at"] > command_deadline:
return 0.0, 0.0, "stale-command", result
decision = result["decision"]
return decision["speed_mps"], decision["yaw_rate_rps"], "none", result
def _run(self):
model = None
policy_epoch = None
self.directory.mkdir(parents=True, exist_ok=True)
try:
with (self.directory / "decisions.jsonl").open("a", encoding="utf-8") as journal:
while True:
with self.condition:
self.condition.wait_for(
lambda: self.closed or (self.enabled and self.pending is not None)
)
if self.closed:
return
rgb, frame_id, captured_at, sim_ns, epoch, observation = self.pending
self.pending = None
try:
if model is None:
model = self.factory()
model.ready()
started = self.clock()
evidence = {}
if self.policy is None:
if policy_epoch is not None and policy_epoch != epoch:
model.reset()
policy_epoch = epoch
decision, boxes, evidence = model.infer_observation(
rgb, observation, frame_id
)
else:
road, boxes = model.infer(rgb)
with self.condition:
if epoch != self.epoch or not self.enabled:
continue
if self.policy is not None and policy_epoch != epoch:
self.policy.reset()
policy_epoch = epoch
if self.policy is not None:
decision = self.policy.decide(road, boxes).model_dump()
completed = self.clock()
record = dict(
frame_id=frame_id,
captured_at=captured_at,
simulation_time_ns=sim_ns,
completed_at=completed,
inference_ms=(completed - started) * 1000,
decision=decision,
)
with self.condition:
self.ready, self.ever_ready, self.error = True, True, None
if epoch != self.epoch or not self.enabled:
continue # A pause/resume fences every earlier in-flight decision.
self.result = record
self.count += 1
# Archive source RGB on Worker, independently of the video stream.
from PIL import Image
image_path = self.directory / f"{frame_id:08d}.jpg"
Image.fromarray(rgb).save(image_path, "JPEG", quality=90)
record = {
**record,
"image": image_path.name,
"image_sha256": hashlib.sha256(image_path.read_bytes()).hexdigest(),
"obstacles": boxes,
**evidence,
}
journal.write(json.dumps(record, allow_nan=False) + "\n")
journal.flush()
except Exception as exc:
with (self.directory / "errors.jsonl").open(
"a", encoding="utf-8"
) as errors:
errors.write(
json.dumps(
{
"frame_id": frame_id,
"monotonic": self.clock(),
"error": str(exc),
"type": type(exc).__name__,
"traceback": traceback.format_exc(),
}
)
+ "\n"
)
with self.condition:
self.error, self.ready, self.result = type(exc).__name__, False, None
if model is not None:
model.close()
model = None
with self.condition:
self.condition.wait(timeout=0.5)
finally:
if model is not None:
model.close()
def close(self):
with self.condition:
self.closed = True
self.condition.notify_all()
self.thread.join(timeout=12)
+377
View File
@@ -0,0 +1,377 @@
"""Persistent host coordinator. Control/telemetry I/O never runs in the simulator.
The Windows task owns the coordinator; a Job Object fences its native children.
Closing the viewer or the private control tunnel cannot stop the physics clock.
Runs are bounded locally; reconnect reconciles the same identity, without replay.
"""
import argparse
import ctypes
import hashlib
import json
import logging
import os
import subprocess
import threading
import time
from pathlib import Path
from uuid import uuid4
from core_client import CoreClient
from local_state import StateChannel, read_json, write_json
from model_stack import ROOT, ModelStack, docker, sha256
from worker import terminate_episode
class NativeJob:
"""Windows kills all native descendants if the coordinator unexpectedly exits."""
def __init__(self):
from ctypes import wintypes
self.kernel = ctypes.WinDLL("kernel32", use_last_error=True)
self.kernel.CreateJobObjectW.restype = wintypes.HANDLE
self.kernel.CreateJobObjectW.argtypes = [ctypes.c_void_p, wintypes.LPCWSTR]
self.kernel.SetInformationJobObject.argtypes = [
wintypes.HANDLE,
ctypes.c_int,
ctypes.c_void_p,
wintypes.DWORD,
]
self.kernel.AssignProcessToJobObject.argtypes = [wintypes.HANDLE, wintypes.HANDLE]
self.kernel.CloseHandle.argtypes = [wintypes.HANDLE]
self.handle = self.kernel.CreateJobObjectW(None, None)
# JOBOBJECT_EXTENDED_LIMIT_INFORMATION: 144 bytes on 64-bit Windows;
# BasicLimitInformation.LimitFlags is DWORD at offset 16.
limits = ctypes.create_string_buffer(144)
ctypes.c_uint32.from_buffer(limits, 16).value = 0x2000 # KILL_ON_JOB_CLOSE
if not self.handle or not self.kernel.SetInformationJobObject(self.handle, 9, limits, 144):
raise ctypes.WinError(ctypes.get_last_error())
def assign(self, child):
if not self.kernel.AssignProcessToJobObject(self.handle, int(child._handle)):
terminate_episode(child)
raise ctypes.WinError(ctypes.get_last_error())
def close(self):
if self.handle:
self.kernel.CloseHandle(self.handle)
self.handle = None
def acquire_service_lock(path):
import msvcrt
stream = path.open("a+b")
stream.seek(0)
if not stream.read(1):
stream.write(b"0")
stream.flush()
stream.seek(0)
msvcrt.locking(stream.fileno(), msvcrt.LK_NBLCK, 1)
return stream
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--core", default="http://127.0.0.1:18080")
parser.add_argument("--token-file", type=Path, required=True)
parser.add_argument("--state", type=Path, required=True)
parser.add_argument("--isaac", type=Path, required=True)
parser.add_argument("--stream-address", required=True)
args = parser.parse_args()
args.state.mkdir(parents=True, exist_ok=True)
lock = acquire_service_lock(args.state / "realtime-service.lock")
logging.basicConfig(
filename=args.state / "realtime-service.log",
level=logging.INFO,
format="%(asctime)s %(levelname)s %(message)s",
)
identity_path = args.state / "realtime-identity.json"
identity = read_json(identity_path)
release = sha256(ROOT / "realtime_worker.py")
active_path = args.state / "active.json"
if identity is None or (identity["release"] != release and not active_path.exists()):
identity = {"instance_id": uuid4().hex, "release": release}
write_json(identity_path, identity)
instance = identity["instance_id"]
client = CoreClient(args.core, args.token_file, instance)
stack = ModelStack()
stack.preflight()
sources = {
"worker": release,
"scene": hashlib.sha256(
"".join(
sha256(ROOT / n)
for n in (
"run_realtime.py",
"motion_control.py",
"realtime_ai.py",
"local_state.py",
"terrain.py",
"spawn_clearance.py",
"prepare_terrain.py",
"navigation_client.py",
"navigation/server.py",
"navigation/footprint.py",
"navigation/fastdds.xml",
"navigation/Prepare-Terrain.ps1",
)
).encode()
+ "".join(
sha256(ROOT.parents[1] / "src/k1link/simulation/ai_polygon" / n)
for n in (
"inference.py",
"policy.py",
"contracts.py",
"composition.py",
"terrain_contract.py",
)
).encode()
+ "".join(
sha256(ROOT.parents[1] / "src/k1link" / n)
for n in (
"artifacts.py",
"observatory/__init__.py",
"observatory/modular_composition.py",
"simulation/__init__.py",
)
).encode()
).hexdigest(),
"models": hashlib.sha256(
"".join(
sha256(ROOT / name)
for name in (
"models.worker-006.json",
"model_stack.py",
"compose.models.yaml",
"ddrnet_server.py",
"segformer/server.py",
)
).encode()
).hexdigest(),
"robot": sha256(ROOT / "rover_profile.py"),
}
hello = {
"worker_id": "worker-006-ai-polygon",
"instance_id": instance,
"runtime": "isaac-sim-6.1",
"execution_modes": ["realtime"],
"model_ids": [m["id"] for m in stack.profile["models"]]
+ [stack.profile["navigation"]["id"]],
"profile_sha256": hashlib.sha256(json.dumps(sources, sort_keys=True).encode()).hexdigest(),
"runtime_sources": sources,
"stream": {
"server": args.stream_address,
"signaling_port": 49100,
"media_port": 47998,
"width": 1280,
"height": 720,
"fps": 30,
},
}
write_json(
args.state / "service-status.json",
{"pid": os.getpid(), "instance_id": instance, "hello": hello},
)
def connect():
while True:
try:
client.request("/worker/register", hello)
return
except Exception as exc:
logging.warning("Control unavailable: %s", type(exc).__name__)
time.sleep(3)
# A previous process died. Its Job Object has killed native children.
# Reconcile only exact persisted container IDs; never infer release from a lease.
orphan = read_json(active_path)
if orphan:
if orphan.get("instance_id") != instance:
raise RuntimeError("Reconciliation identity mismatch")
for cid in orphan.get("model_container_ids", []):
record = docker("container", "inspect", cid, check=False)
if record.returncode == 0:
data = json.loads(record.stdout)[0]
if data["Config"]["Labels"].get("com.nodedc.stack") != "ai-polygon":
raise RuntimeError("Reconciliation resource owner changed")
stack.ids.append(cid)
stack.stop()
connect()
client.request(
"/worker/runs/" + orphan["run_id"] + "/finish",
{
"instance_id": instance,
"outcome": "failed",
"resources_released": True,
"message": "Worker перезапустился. Прогон завершён без повторного запуска.",
},
)
active_path.unlink()
connect()
while True:
try:
polled = client.request("/worker/poll", {"instance_id": instance})
except Exception:
connect()
continue
if polled["action"] == "idle":
time.sleep(0.5)
continue
run = polled["run"]
if run.get("clock") != "realtime":
raise RuntimeError("A realtime worker cannot execute lockstep work")
episode = args.state / run["run_id"]
if episode.exists():
raise RuntimeError("An existing episode must never be replayed")
episode.mkdir()
run_file = episode / "run.json"
write_json(run_file, run)
channel = StateChannel(episode)
channel.write("control", run)
active = {"run_id": run["run_id"], "instance_id": instance, "model_container_ids": []}
write_json(active_path, active)
child = job = starter = None
cancelled = threading.Event()
model_failure = []
result = {"outcome": "failed", "message": "Прогон прерван. Журнал сохранён на Worker."}
try:
cache = args.state / "worlds"
cache.mkdir(exist_ok=True)
source = cache / (run["world"]["sha256"] + ".ply")
client.download(run["world"], source)
client.request("/worker/runs/" + run["run_id"] + "/progress", {"phase": "scene"})
from prepare_terrain import prepare_terrain
job = NativeJob()
def preparation_pulse(run=run):
desired = client.request(
"/worker/poll", {"instance_id": instance, "run_id": run["run_id"]}
)
if desired["run"]["control"] == "stop":
raise InterruptedError("Terrain preparation cancelled")
terrain_path = prepare_terrain(
args.state.parent, episode, run["world"], job, preparation_pulse
)
run["terrain_manifest"] = str(terrain_path)
write_json(run_file, run)
with (episode / "isaac.log").open("wb") as output:
child = subprocess.Popen(
[
str(args.isaac / "python.bat"),
str(ROOT / "run_realtime.py"),
"--run",
str(run_file),
"--source",
str(source),
"--stream-address",
args.stream_address,
],
stdout=output,
stderr=subprocess.STDOUT,
)
job.assign(child)
def acquired(ids, active=active):
active["model_container_ids"] = ids
write_json(active_path, active)
def start_models(
cancelled=cancelled, model_failure=model_failure, acquired=acquired, run=run
):
try:
stack.start(
cancelled=cancelled.is_set,
on_acquired=acquired,
selection=run["request"].get("composition"),
)
except InterruptedError:
pass
except Exception as exc:
model_failure.append(type(exc).__name__)
logging.exception("Model startup failed")
stop_deadline = None
deadline = time.monotonic() + 300 + run["request"]["duration_seconds"]
last_snapshot = -1
registered = True
while child.poll() is None:
now = time.monotonic()
if now >= deadline or (stop_deadline and now >= stop_deadline):
terminate_episode(child)
break
if model_failure:
raise RuntimeError("Local model startup failed")
try:
if not registered:
client.request("/worker/register", hello)
registered = True
polled = client.request(
"/worker/poll", {"instance_id": instance, "run_id": run["run_id"]}
)
desired = polled["run"]
channel.write("control", desired)
if desired["control"] == "stop":
cancelled.set()
stop_deadline = stop_deadline or now + 15
if desired["control"] == "play" and starter is None:
starter = threading.Thread(
target=start_models, name="polygon-model-start", daemon=True
)
starter.start()
snapshot = channel.read("snapshot")
if snapshot and snapshot["sequence"] > last_snapshot:
client.request("/worker/runs/" + run["run_id"] + "/snapshot", snapshot)
last_snapshot = snapshot["sequence"]
except Exception as exc:
registered = False
logging.warning(
"Episode control/telemetry disconnected: %s", type(exc).__name__
)
time.sleep(0.25)
result = read_json(episode / "result.json", result)
if cancelled.is_set():
result = {"outcome": "stopped", "message": "Движение и inference остановлены."}
except InterruptedError:
result = {"outcome": "stopped", "message": "Подготовка симуляции остановлена."}
except Exception:
logging.exception("Episode failed")
finally:
channel.close()
cancelled.set()
if child is not None and child.poll() is None:
terminate_episode(child)
if job is not None:
job.close()
if starter is not None:
starter.join(timeout=150)
if starter.is_alive():
raise RuntimeError("Model startup has not released ownership")
stack.stop()
active["resources_released"] = True
write_json(active_path, active)
write_json(episode / "worker-result.json", result)
while True:
try:
client.request("/worker/register", hello)
client.request(
"/worker/runs/" + run["run_id"] + "/finish",
{**result, "instance_id": instance, "resources_released": True},
)
active_path.unlink()
break
except Exception:
logging.warning("Finish pending reconciliation")
time.sleep(3)
lock.close()
if __name__ == "__main__":
try:
main()
except Exception:
logging.exception("Simulation coordinator failed")
raise
@@ -0,0 +1,105 @@
"""Admit validated Worker-local Gaussian + collision assets, sending metadata only.
Run with the prepared Isaac Python environment on the Worker. The descriptor
declares hashes, provenance, size and scene settings; registration does not
assert that AI has completed a route. No model/profile is changed here.
"""
import argparse
import hashlib
import json
import os
import shutil
import sys
from pathlib import Path
sys.path.insert(0, str(Path(__file__).resolve().parents[2] / "src"))
from core_client import CoreClient
from local_state import read_json, write_json
from terrain import load_glb
from k1link.simulation.ai_polygon.contracts import WorkerWorldCreate
from k1link.simulation.ai_polygon.terrain_contract import terrain_matches
from k1link.simulation.ai_polygon.worlds import inspect_gaussian_ply
def digest(path):
with path.open("rb") as stream:
return hashlib.file_digest(stream, "sha256").hexdigest()
def preserve(source, target, expected):
if target.exists():
if digest(target) != expected:
raise ValueError("Existing immutable asset identity differs")
return
temporary = target.with_suffix(target.suffix + ".part")
try:
os.link(source, temporary)
except OSError:
shutil.copyfile(source, temporary)
if digest(temporary) != expected:
raise ValueError("Asset identity changed while staging")
temporary.replace(target)
def stage(root, source, collider, descriptor):
if (root / "state/active.json").exists():
raise RuntimeError("Finish the active simulation before importing a scene")
request = WorkerWorldCreate.model_validate(descriptor)
if (
source.stat().st_size != request.byte_length
or digest(source) != request.sha256
or inspect_gaussian_ply(source) != request.splat_count
or digest(collider) != request.collider_sha256
):
raise ValueError("Paired asset does not match its declared identity")
load_glb(collider) # Reject unsupported transforms, external buffers and non-finite data.
cache = root / "state/worlds"
cache.mkdir(parents=True, exist_ok=True)
preserve(source, cache / (request.sha256 + ".ply"), request.sha256)
directory = root / "assets/prepared-worlds" / request.sha256
directory.mkdir(parents=True, exist_ok=True)
destination = directory / "collision.glb"
preserve(collider, destination, request.collider_sha256)
manifest = {
"schema_version": "missioncore.ai-polygon-terrain/v1",
"source_sha256": request.sha256,
"settings": request.settings.model_dump(mode="json"),
"generator": "paired-source",
"collider": str(destination),
"collider_sha256": request.collider_sha256,
"source_url": str(request.source_url) if request.source_url else None,
"qualification": "paired-source-requires-contact-and-route-validation",
}
path = directory / "terrain.json"
if path.exists():
existing = read_json(path)
if existing.get("collider_sha256") != request.collider_sha256 or not terrain_matches(
existing, request.model_dump(mode="json")
):
raise ValueError("Existing paired-scene calibration differs")
else:
write_json(path, manifest)
return request
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--root", type=Path, required=True)
parser.add_argument("--source", type=Path, required=True)
parser.add_argument("--collider", type=Path, required=True)
parser.add_argument("--descriptor", type=Path, required=True)
parser.add_argument("--core", default="http://127.0.0.1:18080")
args = parser.parse_args()
request = stage(args.root, args.source, args.collider, read_json(args.descriptor))
identity = read_json(args.root / "state/realtime-identity.json")
client = CoreClient(args.core, args.root / "private/worker.token", identity["instance_id"])
result = client.request("/worker/worlds", request.model_dump(mode="json"))
write_json(args.root / "assets/prepared-worlds" / request.sha256 / "registration.json", result)
print(json.dumps(result, ensure_ascii=True))
if __name__ == "__main__":
main()
@@ -0,0 +1,10 @@
FROM ros:humble-ros-base-jammy
LABEL com.nodedc.product="mission-core" \
com.nodedc.stack="ai-polygon-route-author" \
com.nodedc.role="offline-route-author" \
com.nodedc.managed-by="ai-polygon-worker"
RUN apt-get update && apt-get install -y --no-install-recommends g++ librecast-dev \
&& rm -rf /var/lib/apt/lists/*
COPY habitat_route.cpp /src/habitat_route.cpp
RUN g++ -O2 -std=c++17 -I/usr/include/recastnavigation /src/habitat_route.cpp -lDetour -o /usr/local/bin/habitat-route
ENTRYPOINT ["habitat-route"]
@@ -0,0 +1,25 @@
# Offline Habitat route authoring
This tool uses Ubuntu's packaged Recast/Detour library to extract a complete
route from a supplied Habitat navmesh. It runs on Worker only. It outputs task
waypoints, never runtime collision knowledge for the AI composition.
Build the `Dockerfile` into `ndc-ai-polygon-route-author:habitat-v1` using the
isolated anonymous Docker configuration described in the Worker runbook. Mount
only the source asset directory read-only, disable container networking, and run:
```
habitat-route /data/scene.navmesh startX startY startZ goalX goalY goalZ
```
Coordinates remain Habitat Y-up in the output. Mission Core conversion is
`(x,y,z) -> (x,-z,y)`. The source navmesh's agent radius and step capability may
be incompatible with the 1 x 1 m rover. Extracted points require full-footprint
support/collision checks and a real closed-loop run before acceptance. Partial
paths, incompatible binary layouts, and disconnected endpoints are rejected.
Sources: Habitat-Sim `src/esp/nav/PathFinder.cpp` and `PathFinder.h` define the
version 1/2 binary container; Recast Navigation's Detour performs the actual
path search. The loader supports 32-bit Detour references and the v2 56-byte
settings layout. The image build and source/output hashes belong in the
private qualification receipt. No asset is modified by route extraction.
@@ -0,0 +1,49 @@
// Offline mission authoring using the supplied Habitat/Detour navmesh.
// Binary layout: facebookresearch/habitat-sim src/esp/nav/PathFinder.cpp.
// No navmesh or privileged scene geometry is supplied to runtime inference.
#include <DetourNavMesh.h>
#include <DetourNavMeshQuery.h>
#include <DetourAlloc.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <cmath>
struct Header { int magic, version, tiles; dtNavMeshParams params; };
struct Tile { dtTileRef ref; int size; };
void require(bool ok, const char* message) { if (!ok) throw std::runtime_error(message); }
int main(int argc, char** argv) { try {
require(argc == 8, "navmesh start-x start-y start-z goal-x goal-y goal-z required");
std::ifstream file(argv[1], std::ios::binary); Header h{};
require(bool(file.read(reinterpret_cast<char*>(&h), sizeof(h))), "header read failed");
require(h.magic == (('M'<<24)|('S'<<16)|('E'<<8)|'T') && (h.version==1 || h.version==2), "unsupported navmesh");
require(h.tiles > 0 && h.tiles < 100000, "invalid tile count");
// v2 stores thirteen float settings and four boolean flags (56 bytes).
if (h.version == 2) file.seekg(56, std::ios::cur);
dtNavMesh* mesh = dtAllocNavMesh(); require(mesh, "allocation failed");
require(dtStatusSucceed(mesh->init(&h.params)), "init failed");
for (int i=0;i<h.tiles;i++) {
Tile t{}; require(bool(file.read(reinterpret_cast<char*>(&t),sizeof(t))), "tile header failed");
require(t.ref && t.size>0 && t.size<100000000, "invalid tile");
auto* data=static_cast<unsigned char*>(dtAlloc(t.size,DT_ALLOC_PERM));
require(data && bool(file.read(reinterpret_cast<char*>(data),t.size)), "tile read failed");
require(dtStatusSucceed(mesh->addTile(data,t.size,DT_TILE_FREE_DATA,t.ref,nullptr)), "tile version mismatch");
}
require(file.peek()==std::char_traits<char>::eof(), "unexpected trailing bytes");
dtNavMeshQuery query; require(dtStatusSucceed(query.init(mesh,65535)), "query init failed");
float start[3],goal[3],nearStart[3],nearGoal[3],extent[3]={2,4,2};
for(int i=0;i<3;i++){start[i]=std::stof(argv[i+2]);goal[i]=std::stof(argv[i+5]);}
dtQueryFilter filter; dtPolyRef first=0,last=0;
require(dtStatusSucceed(query.findNearestPoly(start,extent,&filter,&first,nearStart)) && first,"start outside navmesh");
require(dtStatusSucceed(query.findNearestPoly(goal,extent,&filter,&last,nearGoal)) && last,"goal outside navmesh");
dtPolyRef path[4096];int count=0;
auto status=query.findPath(first,last,nearStart,nearGoal,&filter,path,&count,4096);
require(dtStatusSucceed(status) && !dtStatusDetail(status,DT_BUFFER_TOO_SMALL) && count && path[count-1]==last,"no complete route");
float points[4096*3];int n=0;
status=query.findStraightPath(nearStart,nearGoal,path,count,points,nullptr,nullptr,&n,4096,DT_STRAIGHTPATH_ALL_CROSSINGS);
require(dtStatusSucceed(status) && !dtStatusDetail(status,DT_BUFFER_TOO_SMALL) && n>=2,"route extraction failed");
double length=0;for(int i=1;i<n;i++){double d=0;for(int j=0;j<3;j++)d+=std::pow(points[i*3+j]-points[(i-1)*3+j],2);length+=std::sqrt(d);}
std::cout<<std::setprecision(10)<<"{\"length_m\":"<<length<<",\"points_habitat\":[";
for(int i=0;i<n;i++){if(i)std::cout<<',';std::cout<<'['<<points[i*3]<<','<<points[i*3+1]<<','<<points[i*3+2]<<']';}
std::cout<<"]}"<<std::endl;dtFreeNavMesh(mesh);return 0;
} catch(const std::exception& e){std::cerr<<e.what()<<std::endl;return 1;} }
+145
View File
@@ -0,0 +1,145 @@
"""Metric 1 x 1 m, four-wheel differential laboratory chassis.
Geometry is explicit. Mass/friction/drive gains are provisional simulation
parameters, NOT a calibrated braking or terrain model of the owner's vehicles.
"""
PROFILE = {
"schema_version": "missioncore.virtual-rover/v1",
"length_m": 1.0,
"width_m": 1.0,
"wheel_radius_m": 0.25,
"wheel_collision": "physx-convex-cylinder",
"wheelbase_m": 0.5,
"body_contact_height_m": 0.37,
"track_width_m": 0.9,
"camera_forward_m": 0.38, # Lens ahead of the mast's front face (x=0.33).
"camera_pitch_degrees": -10.0,
"rear_range_forward_m": -0.38,
"range_sensor_profile": "front-360-rear-160-first-hit-v2",
"camera_focal_length_mm": 14.0, # Wide pinhole view retains ground on slopes.
"camera_horizontal_aperture_mm": 36.0,
"mass_kg": 24.0,
"calibration": "geometry-only",
"max_step_m": 0.10,
"max_slope_degrees": 25.0,
"stop_tilt_degrees": 30.0,
"drive_damping": 1000.0,
"brake_stiffness": 2000.0,
"brake_damping": 100.0,
"solver_position_iterations": 32,
"solver_velocity_iterations": 8,
"contact_offset_m": 0.005,
"wheel_names": ["front_left", "rear_left", "front_right", "rear_right"],
}
class DifferentialDrive:
"""Velocity drive plus physical wheel-position hold when braking.
Holding uses joint drive forces, never teleportation or a frozen chassis.
The same actuator is used by capability qualification and the live scene.
"""
def __init__(self, articulation):
self.robot = articulation
self.indices = articulation.get_dof_indices(PROFILE["wheel_names"])
self.holding = None
def command(self, velocity, yaw_rate=0.0):
hold = abs(velocity) + abs(yaw_rate) < 1e-5
if hold != self.holding:
if hold:
position = self.robot.get_dof_positions(dof_indices=self.indices)
self.robot.set_dof_position_targets(position, dof_indices=self.indices)
self.robot.set_dof_gains(
stiffnesses=PROFILE["brake_stiffness"] if hold else 0.0,
dampings=PROFILE["brake_damping"] if hold else PROFILE["drive_damping"],
dof_indices=self.indices,
)
self.holding = hold
half_track, radius = PROFILE["track_width_m"] / 2, PROFILE["wheel_radius_m"]
left = (velocity - yaw_rate * half_track) / radius
right = (velocity + yaw_rate * half_track) / radius
self.robot.set_dof_velocity_targets([[left, left, right, right]], dof_indices=self.indices)
def create_rover(stage, spawn, heading, ground_normal=(0, 0, 1), root_path="/World/Rover"):
import carb.settings
from omni.physx.bindings._physx import SETTING_COLLISION_APPROXIMATE_CYLINDERS
from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics, UsdShade
# Official PhysX cylinder approximation avoids the pathological contact
# cost measured on the Forest reconstruction. This changes collision
# representation, not wheel radius, chassis dimensions or terrain limits.
carb.settings.get_settings().set_bool(SETTING_COLLISION_APPROXIMATE_CYLINDERS, True)
tire = UsdShade.Material.Define(stage, "/World/Materials/RoverTire")
tire_physics = UsdPhysics.MaterialAPI.Apply(tire.GetPrim())
tire_physics.CreateStaticFrictionAttr(1.0)
tire_physics.CreateDynamicFrictionAttr(0.9)
tire_physics.CreateRestitutionAttr(0)
root = UsdGeom.Xform.Define(stage, root_path)
root.AddTranslateOp().Set(Gf.Vec3d(*spawn))
tilt = Gf.Rotation(Gf.Vec3d(0, 0, 1), Gf.Vec3d(*ground_normal)).GetQuat()
yaw = Gf.Rotation(Gf.Vec3d(0, 0, 1), heading).GetQuat()
root.AddOrientOp().Set(Gf.Quatf(tilt * yaw))
UsdPhysics.ArticulationRootAPI.Apply(root.GetPrim())
articulation = PhysxSchema.PhysxArticulationAPI.Apply(root.GetPrim())
articulation.CreateEnabledSelfCollisionsAttr(False)
articulation.CreateSolverPositionIterationCountAttr(PROFILE["solver_position_iterations"])
articulation.CreateSolverVelocityIterationCountAttr(PROFILE["solver_velocity_iterations"])
def contact(prim):
UsdPhysics.CollisionAPI.Apply(prim)
collision = PhysxSchema.PhysxCollisionAPI.Apply(prim)
collision.CreateContactOffsetAttr(PROFILE["contact_offset_m"])
collision.CreateRestOffsetAttr(0.0)
body = UsdGeom.Xform.Define(stage, root_path + "/chassis")
body.AddTranslateOp().Set(Gf.Vec3d(0, 0, 0.12))
UsdPhysics.RigidBodyAPI.Apply(body.GetPrim())
PhysxSchema.PhysxRigidBodyAPI.Apply(body.GetPrim()).CreateMaxDepenetrationVelocityAttr(1.0)
UsdPhysics.MassAPI.Apply(body.GetPrim()).CreateMassAttr(20)
hull = UsdGeom.Cube.Define(stage, root_path + "/chassis/hull")
hull.CreateSizeAttr(1)
hull.AddScaleOp().Set(Gf.Vec3f(0.8, 0.78, 0.24))
hull.CreateDisplayColorAttr([Gf.Vec3f(0.65, 0.67, 0.61)])
contact(hull.GetPrim())
# Mast is visual geometry under the rigid chassis (not a separate body).
sensor = UsdGeom.Cube.Define(stage, root_path + "/chassis/sensor")
sensor.CreateSizeAttr(1)
sensor.AddTranslateOp().Set(Gf.Vec3d(0.25, 0, 0.3))
sensor.AddScaleOp().Set(Gf.Vec3f(0.16, 0.16, 0.6))
sensor.CreateDisplayColorAttr([Gf.Vec3f(0.12, 0.13, 0.13)])
for name in PROFILE["wheel_names"]:
x = PROFILE["wheelbase_m"] / 2 * (1 if name.startswith("front") else -1)
y = 0.45 if name.endswith("left") else -0.45
path = root_path + "/" + name
wheel = UsdGeom.Cylinder.Define(stage, path)
wheel.CreateAxisAttr("Y")
wheel.CreateRadiusAttr(PROFILE["wheel_radius_m"])
wheel.CreateHeightAttr(0.1)
wheel.AddTranslateOp().Set(Gf.Vec3d(x, y, 0))
wheel.CreateDisplayColorAttr([Gf.Vec3f(0.08, 0.08, 0.08)])
UsdPhysics.RigidBodyAPI.Apply(wheel.GetPrim())
PhysxSchema.PhysxRigidBodyAPI.Apply(wheel.GetPrim()).CreateMaxDepenetrationVelocityAttr(1.0)
contact(wheel.GetPrim())
UsdShade.MaterialBindingAPI.Apply(wheel.GetPrim()).Bind(
tire, UsdShade.Tokens.weakerThanDescendants, "physics"
)
UsdPhysics.MassAPI.Apply(wheel.GetPrim()).CreateMassAttr(1)
joint = UsdPhysics.RevoluteJoint.Define(stage, root_path + "/joints/" + name)
joint.CreateBody0Rel().SetTargets([body.GetPath()])
joint.CreateBody1Rel().SetTargets([wheel.GetPath()])
joint.CreateLocalPos0Attr(Gf.Vec3f(x, y, -0.12))
joint.CreateLocalPos1Attr(Gf.Vec3f(0))
joint.CreateAxisAttr("Y")
drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular")
drive.CreateTypeAttr("force")
drive.CreateStiffnessAttr(0)
drive.CreateDampingAttr(PROFILE["drive_damping"])
drive.CreateMaxForceAttr(50)
drive.CreateTargetVelocityAttr(0)
return PROFILE
+566
View File
@@ -0,0 +1,566 @@
"""Native Worker-local physics/render/camera loop, independent of Core and AI latency."""
import argparse
import json
import math
import shutil
import sys
import time
import traceback
from datetime import UTC, datetime
from pathlib import Path
from local_state import StateChannel, read_json, write_json
from motion_control import CONTROL_PROFILE, DriveEnvelope
from realtime_ai import LatestInference
from rover_profile import PROFILE, DifferentialDrive, create_rover
parser = argparse.ArgumentParser()
parser.add_argument("--run", type=Path, required=True)
parser.add_argument("--source", type=Path, required=True)
parser.add_argument("--stream-address", required=True)
args, _ = parser.parse_known_args()
root = Path(__file__).resolve().parents[2]
sys.path.insert(0, str(root / "src"))
run = read_json(args.run)
settings = run["world"]["settings"]
directory = args.run.parent
channel = StateChannel(directory)
result = {"outcome": "failed", "message": "Симуляция не завершена."}
app = ai = None
physics_callbacks = []
def startup_phase(phase):
record = {"phase": phase, "utc": datetime.now(UTC).isoformat(), "monotonic": time.monotonic()}
write_json(directory / "startup.json", record)
print(json.dumps({"startup": record}), flush=True)
try:
startup_phase("native-runtime")
from isaacsim import SimulationApp
app = SimulationApp(
{
"headless": True,
"hide_ui": True,
"multi_gpu": False,
"width": 1280,
"height": 720,
"window_width": 1280,
"window_height": 720,
"renderer": "RaytracedLighting",
"display_options": 0,
"extra_args": [
"--/app/player/useFixedTimeStepping=false",
"--/app/runLoops/main/manualModeEnabled=false",
"--/exts/isaacsim.core.throttling/enable_manualmode=false",
],
}
)
import carb.settings
import numpy as np
import omni.kit.app
import omni.replicator.core as rep
import omni.timeline
import omni.usd
from isaacsim.core.experimental.prims import Articulation
from isaacsim.core.experimental.utils.app import enable_extension
from isaacsim.core.simulation_manager import SimulationEvent, SimulationManager
from navigation.footprint import FRAME_DEADLINE_SECONDS
from navigation_client import ComposedInference
from omni.kit.loop import _loop as omni_loop
from omni.kit.viewport.utility import get_active_viewport
from pxr import Gf, UsdGeom, UsdLux
from terrain import RangeSensor, install_terrain
from k1link.simulation.ai_polygon.mission_policy import inclination
startup_phase("stream-and-gaussian-runtime")
config = carb.settings.get_settings()
prefix = "/exts/omni.kit.livestream.app/primaryStream/"
for key, value in {
"publicIp": args.stream_address,
"signalPort": 49100,
"streamPort": 47998,
"targetFps": 30,
"enableEventTracing": False,
}.items():
config.set(prefix + key, value)
enable_extension("omni.kit.livestream.app")
enable_extension("omni.kit.converter.gsplat")
from usd_convert_gsplat import read_ply, write_gaussian_splat_usd
converted = args.source.with_suffix(".usd")
if not converted.exists():
temporary = converted.with_suffix(".part.usd")
write_gaussian_splat_usd(
read_ply(str(args.source)),
str(temporary),
source_file=str(args.source),
prim_name="Gaussians",
up_axis="Z",
)
temporary.replace(converted)
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
visual = UsdGeom.Xform.Define(stage, "/World/Scan")
visual.AddRotateXYZOp().Set(Gf.Vec3f(*settings["rotation_degrees"]))
visual.AddScaleOp().Set(Gf.Vec3f(settings["meters_per_unit"]))
stage.DefinePrim("/World/Scan/Gaussians").GetReferences().AddReference(str(converted))
startup_phase("terrain")
wheel_center_z, terrain_record = install_terrain(stage, run["terrain_manifest"], run["world"])
write_json(directory / "terrain.json", terrain_record)
UsdLux.DomeLight.Define(stage, "/World/Light").CreateIntensityAttr(500)
create_rover(
stage,
[*settings["spawn_xy"], wheel_center_z],
settings["heading_degrees"],
terrain_record["initial_ground_normal"],
)
write_json(directory / "robot-profile.json", PROFILE)
write_json(directory / "motion-control.json", CONTROL_PROFILE)
startup_phase("cameras")
def camera(path, aspect, focal_length=24):
value = UsdGeom.Camera.Define(stage, path)
value.CreateFocalLengthAttr(focal_length)
value.CreateHorizontalApertureAttr(36)
value.CreateVerticalApertureAttr(36 / aspect)
value.CreateClippingRangeAttr(Gf.Vec2f(0.05, 1000))
return value, value.AddTranslateOp(), value.AddOrientOp()
sensor, sensor_pos, sensor_rot = camera(
"/World/Sensor", 4 / 3, PROFILE["camera_focal_length_mm"]
)
focal_pixels = (
800 * PROFILE["camera_focal_length_mm"] / PROFILE["camera_horizontal_aperture_mm"]
)
observer, observer_pos, observer_rot = camera("/World/Observer", 16 / 9)
product = rep.create.render_product(str(sensor.GetPath()), (800, 600))
annotator = rep.AnnotatorRegistry.get_annotator("rgb")
annotator.attach(product)
config.set("/omni/replicator/captureOnPlay", True)
viewport = get_active_viewport()
if viewport is None:
raise RuntimeError("Streaming viewport is unavailable")
viewport.camera_path = str(observer.GetPath())
viewport.set_texture_resolution((1280, 720))
# Initialise the final camera before the first rendered frames. Starting
# WebRTC with an uninitialised observer and an already paused timeline can
# leave the native stream without its first image/offer.
initial = Gf.Vec3d(*settings["spawn_xy"], wheel_center_z + 0.12)
heading = math.radians(settings["heading_degrees"])
initial_eye = initial + Gf.Vec3d(-math.cos(heading) * 2.5, -math.sin(heading) * 2.5, 1.5)
observer_pos.Set(initial_eye)
observer_rot.Set(
Gf.Quatf(
Gf.Matrix4d()
.SetLookAt(initial_eye, initial + Gf.Vec3d(0.1, 0, 0.2), Gf.Vec3d(0, 0, 1))
.GetInverse()
.ExtractRotationQuat()
)
)
startup_phase("physics-setup")
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
timeline = omni.timeline.get_timeline_interface()
# Render rate and physics rate are separate. Physics steps follow elapsed time.
config.set("/app/player/useFixedTimeStepping", False)
config.set("/exts/isaacsim.core.throttling/enable_manualmode", False)
omni_loop.acquire_loop_interface().set_manual_mode(False)
timeline.set_play_every_frame(False)
config.set("/persistent/simulation/minFrameRate", 1)
config.set("/app/runLoops/main/rateLimitEnabled", True)
config.set("/app/runLoops/main/rateLimitFrequency", 30)
startup_phase("initial-render-and-contact")
timeline.play()
for _ in range(30):
app.update()
articulation = Articulation("/World/Rover")
drive = DifferentialDrive(articulation)
wheel_indices = articulation.get_dof_indices(PROFILE["wheel_names"])
def body_transform():
positions, orientations = articulation.get_world_poses()
position, orientation = positions.numpy()[0], orientations.numpy()[0]
transform = Gf.Matrix4d(1)
transform.SetRotate(Gf.Quatd(float(orientation[0]), Gf.Vec3d(*map(float, orientation[1:]))))
transform.SetTranslateOnly(Gf.Vec3d(*map(float, position)))
return transform
settled = body_transform().ExtractTranslation()
write_json(
directory / "initial-contact.json",
{
"pose": list(settled),
"expected_xy": settings["spawn_xy"],
"spawn_contact_z": terrain_record["initial_contact_z"],
},
)
if math.hypot(settled[0] - settings["spawn_xy"][0], settled[1] - settings["spawn_xy"][1]) > 0.2:
raise RuntimeError("Rover start is unstable on this reconstructed surface")
timeline.pause()
app.update()
startup_phase("ready")
baseline = SimulationManager.get_num_physics_steps()
range_sensor = RangeSensor()
from k1link.simulation.ai_polygon.mission_policy import WaypointMission
# The episode owns the mission. Reconnecting a failed model client must not
# rewind its route cursor, recovery budget or latched terminal condition.
mission = WaypointMission(settings.get("route_xy", []))
ai = LatestInference(
lambda: ComposedInference(
root / "simulation/ai-polygon", run, directory / "camera", mission=mission
),
None,
directory / "camera",
)
ai.start()
control = {"control": "pause", "control_sequence": 0, "camera": "follow"}
start = last_report = last_sensor = last_control = time.monotonic()
render_frames = sensor_frames = sequence = 0
prior_metrics = (start, 0, 0, 0, baseline)
speed = 0.0
had_ai = False
unstable = False
terminal_at = None
envelope = DriveEnvelope(track_width=PROFILE["track_width_m"])
state = {"velocity": 0.0, "yaw_rate": 0.0, "stop_reason": "paused", "decision": None}
physics_rows = []
def control_step(dt, _context):
velocity, yaw_rate, reason, decision = ai.command(
time.monotonic(), frame_deadline=FRAME_DEADLINE_SECONDS
)
terminal = unstable or (
decision and decision["decision"]["reason"] in {"unstable", "stuck", "goal-reached"}
)
velocity, yaw_rate = envelope.step(
velocity, yaw_rate, dt, stop=bool(terminal) or reason != "none"
)
drive.command(velocity, yaw_rate)
state.update(
velocity=velocity,
yaw_rate=yaw_rate,
stop_reason="unstable" if unstable else reason,
decision=decision,
)
def update_cameras():
transform = body_transform()
position = transform.ExtractTranslation()
forward = transform.TransformDir(Gf.Vec3d(1, 0, 0)).GetNormalized()
yaw = math.atan2(forward[1], forward[0])
# Both sensor position and attitude follow the articulated body.
# Mounting height is measured from nominal wheel contact.
eye = transform.Transform(
Gf.Vec3d(
PROFILE["camera_forward_m"],
0,
settings["camera_height_m"] - PROFILE["body_contact_height_m"],
)
)
pitch = math.radians(PROFILE["camera_pitch_degrees"])
camera_axes = [
transform.TransformDir(Gf.Vec3d(*axis)).GetNormalized()
for axis in (
(math.cos(pitch), 0, math.sin(pitch)),
(0, 1, 0),
(-math.sin(pitch), 0, math.cos(pitch)),
)
]
def look_at(pos, rotation, origin, target, up=None):
if up is None:
up = Gf.Vec3d(0, 0, 1)
pos.Set(origin)
rotation.Set(
Gf.Quatf(
Gf.Matrix4d().SetLookAt(origin, target, up).GetInverse().ExtractRotationQuat()
)
)
look_at(
sensor_pos,
sensor_rot,
eye,
eye + camera_axes[0],
camera_axes[2],
)
mode = control.get("camera", "follow")
if mode == "camera":
viewport.camera_path = str(sensor.GetPath())
else:
viewport.camera_path = str(observer.GetPath())
offset = (
Gf.Vec3d(-math.cos(yaw) * 2.5, -math.sin(yaw) * 2.5, 1.5)
if mode == "follow"
else Gf.Vec3d(-3, -3, 5)
)
look_at(observer_pos, observer_rot, position + offset, position + Gf.Vec3d(0.1, 0, 0.2))
linear, angular = articulation.get_velocities()
q = transform.ExtractRotationQuat()
state.update(
transform=transform,
position=position,
yaw=yaw,
eye=eye,
camera_axes=camera_axes,
mode=mode,
pose=list(position) + list(q.GetImaginary()) + [q.GetReal()],
speed=float(np.linalg.norm(linear.numpy()[0, :2])),
body_velocity=linear.numpy()[0].tolist(),
body_angular_velocity=angular.numpy()[0].tolist(),
)
def physics_step(dt, _context):
global unstable
update_cameras()
unstable |= inclination(state["pose"]) >= PROFILE["stop_tilt_degrees"]
physics_rows.append(
{
"monotonic": time.monotonic(),
"dt": dt,
"pose": state["pose"],
"velocity": state["body_velocity"],
"angular_velocity": state["body_angular_velocity"],
"command": [state["velocity"], state["yaw_rate"]],
"stop_reason": state["stop_reason"],
}
)
callback_errors = []
def guarded(callback):
def invoke(dt, context):
try:
callback(dt, context)
except Exception:
# Native event dispatch logs Python exceptions and continues.
# A failed control/camera callback must instead end this run.
callback_errors.append(traceback.format_exc())
ai.enable(False)
drive.command(0.0, 0.0)
return invoke
update_cameras()
physics_callbacks.extend(
[
SimulationManager.register_callback(
guarded(control_step), SimulationEvent.PHYSICS_PRE_STEP
),
SimulationManager.register_callback(
guarded(physics_step), SimulationEvent.PHYSICS_POST_STEP
),
]
)
with (
(directory / "motion.jsonl").open("a", encoding="utf-8") as motion,
(directory / "physics-motion.jsonl").open("a", encoding="utf-8") as physics_motion,
):
while app.is_running():
now = time.monotonic()
if now - last_control >= 0.1:
control = channel.read("control", control)
last_control = now
if control["control"] == "stop":
result = {"outcome": "stopped", "message": "Симуляция и inference остановлены."}
break
if now - start >= run["request"]["duration_seconds"]:
result = {"outcome": "completed", "message": "Время прогона завершено."}
break
playing = control["control"] == "play"
ai.enable(playing)
had_ai |= playing
if playing and not timeline.is_playing():
timeline.play()
elif not playing and timeline.is_playing():
timeline.pause()
velocity, yaw_rate, stop_reason, decision = ai.command(
now, frame_deadline=FRAME_DEADLINE_SECONDS
)
transform = body_transform()
q = transform.ExtractRotationQuat()
pose = list(transform.ExtractTranslation()) + list(q.GetImaginary()) + [q.GetReal()]
unstable |= inclination(pose) >= PROFILE["stop_tilt_degrees"]
if unstable:
velocity, yaw_rate, stop_reason = 0.0, 0.0, "unstable"
terminal_reason = (
"unstable" if unstable else (decision["decision"]["reason"] if decision else None)
)
if terminal_reason in {"unstable", "stuck", "goal-reached"}:
velocity, yaw_rate = 0.0, 0.0
terminal_at = now if terminal_at is None else terminal_at
if now - terminal_at >= 2:
result = {
"outcome": "completed" if terminal_reason == "goal-reached" else "failed",
"message": {
"goal-reached": "Ровер достиг цели маршрута.",
"stuck": "Ровер не нашёл проезд после трёх попыток. Прогон завершён.",
"unstable": "Прогон завершён из-за опасного наклона ровера.",
}[terminal_reason],
}
break
# The physics tensor API applies live drive targets in radians/s;
# USD authoring attributes are only the initial scene configuration.
if not playing:
drive.command(*envelope.step(0.0, 0.0, 0.0, stop=True))
update_cameras()
state.update(
velocity=0.0, yaw_rate=0.0, stop_reason="paused", speed=0.0, decision=decision
)
app.update() # Never waits for inference, a network request, or an operator ACK.
if callback_errors:
raise RuntimeError(callback_errors[0])
render_frames += 1
captured_at = time.monotonic()
transform, position = state["transform"], state["position"]
eye, camera_axes = state["eye"], state["camera_axes"]
pose, yaw, speed, mode = state["pose"], state["yaw"], state["speed"], state["mode"]
velocity, yaw_rate = state["velocity"], state["yaw_rate"]
stop_reason, decision = state["stop_reason"], state["decision"]
if physics_rows:
physics_motion.writelines(
json.dumps(row, allow_nan=False) + "\n" for row in physics_rows
)
physics_rows.clear()
physics_motion.flush()
physics = SimulationManager.get_num_physics_steps()
sim_ns = round((physics - baseline) * 1e9 / 60)
if playing and captured_at - last_sensor >= 1 / 5:
rgb = annotator.get_data()
if isinstance(rgb, np.ndarray) and rgb.shape[:2] == (600, 800):
sensor_frames += 1
rear_eye = transform.Transform(
Gf.Vec3d(
PROFILE["rear_range_forward_m"],
0,
settings["camera_height_m"] - PROFILE["body_contact_height_m"],
)
)
points = range_sensor.capture(eye, transform, rear_eye)
quaternion = transform.ExtractRotationQuat()
imaginary = quaternion.GetImaginary()
# Columns are the camera's forward/left/up axes in world.
observation = {
"simulation_time_ns": sim_ns,
"points": points,
"pose": [float(v) for v in position]
+ [float(v) for v in imaginary]
+ [float(quaternion.GetReal())],
"calibration": {
"origin": list(eye),
"rotation": np.array(camera_axes).T.reshape(-1).tolist(),
"intrinsics": [focal_pixels, focal_pixels, 400.0, 300.0],
"body_contact_height_m": PROFILE["body_contact_height_m"],
"range_origins": [list(eye), list(rear_eye)],
},
}
ai.submit(
np.array(rgb[:, :, :3], copy=True, order="C"),
sensor_frames,
captured_at,
sim_ns,
observation,
)
last_sensor = captured_at
if captured_at - last_report >= 0.25:
prior_at, prior_render, prior_sensor, prior_ai, prior_physics = prior_metrics
elapsed = captured_at - prior_at
report = dict(
sequence=sequence,
control_sequence=control["control_sequence"],
state="running" if playing else "paused" if had_ai else "ready",
phase="models" if playing and not ai.ever_ready else "running",
simulation_time_ns=sim_ns,
wall_elapsed_seconds=captured_at - start,
physics_steps=physics - baseline,
render_frames=render_frames,
sensor_frames=sensor_frames,
inference_count=ai.count,
dropped_frames=ai.dropped,
rtf=(physics - prior_physics) / 60 / elapsed,
render_fps=(render_frames - prior_render) / elapsed,
sensor_fps=(sensor_frames - prior_sensor) / elapsed,
ai_hz=(ai.count - prior_ai) / elapsed,
inference_ms=decision["inference_ms"] if decision else None,
frame_age_ms=(captured_at - decision["captured_at"]) * 1000
if decision
else None,
command_age_ms=(captured_at - decision["completed_at"]) * 1000
if decision
else None,
pose_xy=[float(position[0]), float(position[1])],
pose_yaw=yaw,
speed_mps=speed,
applied_speed_mps=velocity,
applied_yaw_rate_rps=yaw_rate,
decision=decision["decision"] if decision else None,
stop_reason=stop_reason,
ai_ready=ai.ready,
stream_ready=True,
camera=mode,
)
channel.write("snapshot", report)
motion.write(
json.dumps(
{
**report,
"monotonic": captured_at,
"pose_xyz_xyzw": pose,
"tilt_degrees": inclination(pose),
"wheel_velocity_rps": articulation.get_dof_velocities(
dof_indices=wheel_indices
)
.numpy()
.tolist()[0],
"usd_position_xyz": list(
UsdGeom.XformCache()
.GetLocalToWorldTransform(
stage.GetPrimAtPath("/World/Rover/chassis")
)
.ExtractTranslation()
),
},
allow_nan=False,
)
+ "\n"
)
motion.flush()
last_report = captured_at
sequence += 1
if elapsed >= 1:
prior_metrics = (captured_at, render_frames, sensor_frames, ai.count, physics)
if shutil.disk_usage(directory).free < 1024**3:
raise RuntimeError("Worker evidence disk is full")
except Exception as exc:
message = str(exc)
placement_failure = any(
text in message
for text in (
"Configured spawn",
"Rover start",
"reconstructed ground",
"reconstructed support",
)
)
result = {
"outcome": "failed",
"message": "Не удалось устойчиво разместить ровер на грунте. Измените положение старта."
if placement_failure
else "Симуляция прервана из-за ошибки на Worker. Журнал сохранён.",
}
traceback.print_exc()
finally:
for callback_id in physics_callbacks:
SimulationManager.deregister_callback(callback_id)
channel.close()
if ai is not None:
ai.close()
write_json(directory / "result.json", result)
if app is not None:
app.close()
+206
View File
@@ -0,0 +1,206 @@
"""One native Isaac episode. Receives pixels from physics; emits virtual commands only."""
import argparse
import base64
import io
import json
import math
import sys
import time
from pathlib import Path
from core_client import CoreClient
parser = argparse.ArgumentParser()
parser.add_argument("--run", type=Path, required=True)
parser.add_argument("--source", type=Path, required=True)
parser.add_argument("--core", required=True)
parser.add_argument("--token-file", type=Path, required=True)
parser.add_argument("--instance", required=True)
args = parser.parse_args()
root = Path(__file__).resolve().parents[2]
sys.path.insert(0, str(root / "src"))
run = json.loads(args.run.read_text())
settings = run["world"]["settings"]
client = CoreClient(args.core, args.token_file, args.instance)
result = {"outcome": "failed", "message": "Симуляция не завершена."}
app = model = robot = controller = None
try:
from isaacsim import SimulationApp
app = SimulationApp({"headless": True, "multi_gpu": False, "width": 800, "height": 600})
import numpy as np
# Isaac bundles the converter compatible with its own USD/ParticleField schema.
import omni.kit.app
import omni.replicator.core as rep
import omni.usd
from isaacsim.core.experimental.utils import app as app_utils
from isaacsim.core.simulation_manager import SimulationManager
from isaacsim.robot.experimental.wheeled_robots.controllers import DifferentialController
from isaacsim.robot.experimental.wheeled_robots.robots import WheeledRobot
from PIL import Image
from pxr import Gf, UsdGeom, UsdLux, UsdPhysics
from k1link.simulation.ai_polygon.inference import ModelInference
from k1link.simulation.ai_polygon.policy import RoadPolicy
omni.kit.app.get_app().get_extension_manager().set_extension_enabled_immediate(
"omni.kit.converter.gsplat", True
)
from usd_convert_gsplat import read_ply, write_gaussian_splat_usd
converted = args.source.with_suffix(".usd")
if not converted.exists():
temporary = converted.with_suffix(".part.usd")
write_gaussian_splat_usd(
read_ply(str(args.source)),
str(temporary),
source_file=str(args.source),
prim_name="Gaussians",
up_axis="Z",
)
temporary.replace(converted)
stage = omni.usd.get_context().get_stage()
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1)
visual = UsdGeom.Xform.Define(stage, "/World/Scan")
visual.AddRotateXYZOp().Set(Gf.Vec3f(*settings["rotation_degrees"]))
visual.AddScaleOp().Set(Gf.Vec3f(settings["meters_per_unit"]))
stage.DefinePrim("/World/Scan/Gaussians").GetReferences().AddReference(str(converted))
# Only a hidden rigid ground plane. Vegetation in the scan is visual-only.
ground = UsdGeom.Plane.Define(stage, "/World/Ground")
ground.CreateAxisAttr("Z")
ground.CreateWidthAttr(20000)
ground.CreateLengthAttr(20000)
ground.AddTranslateOp().Set(Gf.Vec3d(0, 0, settings["ground_z"]))
UsdPhysics.CollisionAPI.Apply(ground.GetPrim())
ground.CreateVisibilityAttr(UsdGeom.Tokens.invisible)
UsdLux.DomeLight.Define(stage, "/World/Light").CreateIntensityAttr(500)
robot_asset = Path("D:/NDC_MISSIONCORE/runtime/simulation/assets/jetbot-6.1-v1/jetbot.usda")
if not robot_asset.is_file():
raise RuntimeError("Prepare the pinned stock Jetbot asset cache first")
heading = math.radians(settings["heading_degrees"])
robot = WheeledRobot(
paths="/World/Rover",
wheel_dof_names=["left_wheel_joint", "right_wheel_joint"],
usd_path=str(robot_asset),
positions=[*settings["spawn_xy"], settings["ground_z"] + 0.05],
orientations=[math.cos(heading / 2), 0, 0, math.sin(heading / 2)],
)
controller = DifferentialController(wheel_radius=0.03, wheel_base=0.1125)
camera = UsdGeom.Camera.Define(stage, "/World/Camera")
camera.CreateFocalLengthAttr(24)
camera.CreateHorizontalApertureAttr(36)
camera.CreateVerticalApertureAttr(27)
camera.CreateClippingRangeAttr(Gf.Vec2f(0.02, 1000))
camera_pos = camera.AddTranslateOp()
camera_rot = camera.AddOrientOp()
render_product = rep.create.render_product(str(camera.GetPath()), (800, 600))
annotator = rep.AnnotatorRegistry.get_annotator("rgb")
annotator.attach(render_product)
SimulationManager.setup_simulation(dt=1 / 60, device="cpu")
app_utils.play()
app_utils.update_app(steps=20)
app_utils.pause()
baseline = SimulationManager.get_num_physics_steps()
profile = json.loads((root / "simulation/ai-polygon/models.worker-006.json").read_text())
model = ModelInference(
"http://127.0.0.1:18092", Path(profile["labels"]), "http://127.0.0.1:18091"
)
model.ready()
policy = RoadPolicy(settings["max_speed_mps"])
def position():
positions, rotations = robot.get_world_poses()
xyz, q = positions.numpy()[0], rotations.numpy()[0]
yaw = math.atan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] ** 2 + q[3] ** 2))
return xyz, yaw
sequence = 0
last_cycle_ms = 0.0
while sequence < run["request"]["max_steps"]:
cycle_started = time.monotonic()
transport_ms = 0.0
transport_started = time.monotonic()
action = client.request(
"/worker/poll", {"instance_id": args.instance, "run_id": run["run_id"]}
)["action"]
transport_ms += (time.monotonic() - transport_started) * 1000
if action == "stop":
result = {"outcome": "stopped", "message": "Прогон остановлен."}
break
if action == "pause":
robot.apply_wheel_actions(controller.forward([0, 0]))
time.sleep(0.1)
continue
if action not in ("play", "step"):
raise RuntimeError("Core withdrew simulation ownership")
xyz, yaw = position()
# Pose is used solely to place the virtual sensor, never passed to policy.
eye = Gf.Vec3d(float(xyz[0]), float(xyz[1]), float(xyz[2] + settings["camera_height_m"]))
camera_pos.Set(eye)
# USD cameras look along local -Z with +Y up. Invert a world-to-camera
# look-at transform to keep the optical horizon level for every heading.
view = Gf.Matrix4d().SetLookAt(
eye, eye + Gf.Vec3d(math.cos(yaw), math.sin(yaw), 0), Gf.Vec3d(0, 0, 1)
)
camera_rot.Set(Gf.Quatf(view.GetInverse().ExtractRotationQuat()))
before = SimulationManager.get_num_physics_steps()
render_started = time.monotonic()
rep.orchestrator.step(rt_subframes=1, delta_time=0.0, pause_timeline=True)
if SimulationManager.get_num_physics_steps() != before:
raise RuntimeError("Rendering advanced the simulation clock")
render_ms = (time.monotonic() - render_started) * 1000
rgb = np.ascontiguousarray(annotator.get_data()[:, :, :3])
started = time.monotonic_ns()
road, boxes = model.infer(rgb)
decision = policy.decide(road, boxes)
inference_ms = (time.monotonic_ns() - started) / 1e6
image = io.BytesIO()
Image.fromarray(rgb).save(image, "JPEG", quality=85)
transport_started = time.monotonic()
client.request(
"/worker/runs/" + run["run_id"] + "/samples",
{
"sequence": sequence,
"simulation_time_ns": sequence * run["step_ns"],
"inference_ms": inference_ms,
"pose_xy": [float(xyz[0]), float(xyz[1])],
"decision": decision.model_dump(),
"image_jpeg_base64": base64.b64encode(image.getvalue()).decode(),
},
)
transport_ms += (time.monotonic() - transport_started) * 1000
robot.apply_wheel_actions(controller.forward([decision.speed_mps, decision.yaw_rate_rps]))
SimulationManager.step(steps=6)
if SimulationManager.get_num_physics_steps() != baseline + (sequence + 1) * 6:
raise RuntimeError("Physics clock left lockstep")
xyz, yaw = position()
transport_started = time.monotonic()
client.request(
"/worker/runs/" + run["run_id"] + "/applied",
{
"sequence": sequence,
"simulation_time_ns": (sequence + 1) * run["step_ns"],
"physics_steps": 6,
"pose_yaw": float(yaw),
"cycle_ms": last_cycle_ms or (time.monotonic() - cycle_started) * 1000,
"render_ms": render_ms,
"transport_ms": transport_ms,
"pose_xy": [float(xyz[0]), float(xyz[1])],
},
)
last_cycle_ms = (time.monotonic() - cycle_started) * 1000
sequence += 1
else:
result = {"outcome": "completed", "message": "Прогон завершён."}
except Exception as exc:
result = {"outcome": "failed", "message": "Прогон прерван. Подробности сохранены на Worker."}
print(type(exc).__name__ + ": " + str(exc), file=sys.stderr, flush=True)
finally:
if model is not None:
model.close()
args.run.with_name("result.json").write_text(json.dumps(result))
if app is not None:
app.close()
@@ -0,0 +1,10 @@
FROM ndc/mission-core-ai-module-ddrnet:20260904-v8
RUN /opt/conda/envs/goose/bin/python -m pip install --no-cache-dir --no-deps \
transformers==4.44.2 tokenizers==0.19.1 safetensors==0.4.5 \
huggingface-hub==0.24.6 regex==2024.9.11 packaging==24.1 \
filelock==3.16.1 fsspec==2024.9.0 PyYAML==6.0.2 \
requests==2.32.3 tqdm==4.66.5 typing-extensions==4.12.2
ENV HF_HUB_OFFLINE=1 TRANSFORMERS_OFFLINE=1 TOKENIZERS_PARALLELISM=false
LABEL com.nodedc.product=mission-core com.nodedc.stack=ai-polygon \
com.nodedc.role=ai-module com.nodedc.managed-by=ai-polygon-worker
ENTRYPOINT ["/opt/conda/envs/goose/bin/python", "-B", "/adapter/segformer/server.py"]
@@ -0,0 +1,37 @@
param([string]$Root = 'D:\NDC_MISSIONCORE\runtime\simulation')
$ErrorActionPreference='Stop'
$ProgressPreference='SilentlyContinue'
[Console]::OutputEncoding=[System.Text.Encoding]::UTF8
$revision='de01bae28967510f9ddd496c60a969357195400c'
$out=Join-Path $Root ('assets\segformer-b2-ade\'+$revision)
New-Item -ItemType Directory -Force $out | Out-Null
$files=@{
'config.json'='ee7400840fdb1e5045f0b2eba78bf053df8e33a309c4acec31705a48c8cc5c00'
'preprocessor_config.json'='8039d1d210abaa7117ad78e58cdfd6141a2ec72c03dae891b3cd76737e422c6c'
'README.md'='7b532a0053fc1769553386090fbc928ed8d0f5b5d5b20cfa8f51e46f56ef3c6d'
'pytorch_model.bin'='187ca07bea003a5717c63d04ea90b07f33cd033c0ebf44b4b89fce5070d6c8f3'
}
foreach($name in $files.Keys) {
$path=Join-Path $out $name
if (!(Test-Path $path)) {
$part=$path+'.part'
Invoke-WebRequest -UseBasicParsing -Uri "https://huggingface.co/nvidia/segformer-b2-finetuned-ade-512-512/resolve/$revision/$name" -OutFile $part
if ((Get-FileHash $part -Algorithm SHA256).Hash.ToLower() -ne $files[$name]) { throw "Downloaded model checksum mismatch: $name" }
Move-Item $part $path
}
if ((Get-FileHash $path -Algorithm SHA256).Hash.ToLower() -ne $files[$name]) { throw "Installed model checksum mismatch: $name" }
}
$ErrorActionPreference='Continue'
$base=docker image inspect ndc/mission-core-ai-module-ddrnet:20260904-v8 --format '{{.Id}}'
if ($LASTEXITCODE -ne 0 -or $base.Trim() -ne 'sha256:a3b7d22f5d3bfdf2d84444b936c8b01abf8243be652387d7e2024ba7bda587f5') {throw 'Pinned base image changed'}
$tag='ndc/mission-core-ai-module-segformer:de01bae2-v1'
$image=docker image inspect $tag --format '{{.Id}}' 2>$null
if ($LASTEXITCODE -ne 0) {
docker build --pull=false --progress plain -t $tag -f (Join-Path $PSScriptRoot 'Dockerfile') $PSScriptRoot
if ($LASTEXITCODE -ne 0) {throw 'SegFormer image build failed'}
$image=docker image inspect $tag --format '{{.Id}}'
if ($LASTEXITCODE -ne 0) {throw 'SegFormer image unavailable'}
}
$receipt=@{schema_version='missioncore.ai-polygon-segformer-install/v1';revision=$revision;image=$image.Trim();tag=$tag;files=$files;assets=$out;installed_at=[DateTime]::UtcNow.ToString('o');license='NVIDIA SegFormer research/evaluation; see upstream model card'}
[IO.File]::WriteAllText((Join-Path $out 'installation.json'),($receipt|ConvertTo-Json -Depth 4),[Text.UTF8Encoding]::new($false))
$receipt|ConvertTo-Json -Depth 4 -Compress
@@ -0,0 +1,52 @@
"""Worker-only offline SegFormer comparison; no live control authority."""
import argparse
import hashlib
import json
import time
from pathlib import Path
import numpy as np
import torch
from PIL import Image
from transformers import SegformerForSemanticSegmentation, SegformerImageProcessor
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()
processor = SegformerImageProcessor.from_pretrained("/assets", local_files_only=True)
model = (
SegformerForSemanticSegmentation.from_pretrained("/assets", local_files_only=True)
.cuda()
.eval()
)
image = Image.open(args.image).convert("RGB").crop((100, 0, 700, 600))
tensor = processor(images=image, return_tensors="pt")["pixel_values"].cuda()
times = []
with torch.inference_mode():
for _ in range(6):
torch.cuda.synchronize()
start = time.monotonic()
logits = model(tensor).logits
logits = torch.nn.functional.interpolate(
logits, size=(512, 512), mode="bilinear", align_corners=False
)
labels = logits.argmax(1)[0].cpu().numpy().astype(np.uint8)
times.append((time.monotonic() - start) * 1000)
args.output.mkdir(parents=True, exist_ok=True)
Image.fromarray(labels).save(args.output / "labels.png")
ids, counts = np.unique(labels, return_counts=True)
report = {
"source_sha256": hashlib.sha256(args.image.read_bytes()).hexdigest(),
"classes": {model.config.id2label[int(i)]: int(counts[n]) for n, i in enumerate(ids)},
"inference_ms": times,
}
(args.output / "report.json").write_text(json.dumps(report, indent=2), encoding="utf-8")
print(json.dumps(report))
if __name__ == "__main__":
main()
+87
View File
@@ -0,0 +1,87 @@
"""Resident, pinned ADE20K surface provider. RGB only; no scene truth input."""
import hashlib
import json
from http.server import BaseHTTPRequestHandler, HTTPServer
from pathlib import Path
import numpy as np
import torch
from PIL import Image
from transformers import SegformerForSemanticSegmentation, SegformerImageProcessor
# These are surface candidates, not permission to drive. The metric terrain
# planner still checks step height, slope and the full rover footprint.
SURFACES = (3, 6, 9, 11, 13, 29, 34, 46, 52, 91)
FILES = {
"config.json": "ee7400840fdb1e5045f0b2eba78bf053df8e33a309c4acec31705a48c8cc5c00",
"preprocessor_config.json": "8039d1d210abaa7117ad78e58cdfd6141a2ec72c03dae891b3cd76737e422c6c",
"pytorch_model.bin": "187ca07bea003a5717c63d04ea90b07f33cd033c0ebf44b4b89fce5070d6c8f3",
}
def main():
for name, expected in FILES.items():
if hashlib.sha256((Path("/assets") / name).read_bytes()).hexdigest() != expected:
raise RuntimeError("SegFormer asset identity changed: " + name)
processor = SegformerImageProcessor.from_pretrained("/assets", local_files_only=True)
model = (
SegformerForSemanticSegmentation.from_pretrained("/assets", local_files_only=True)
.cuda()
.eval()
)
torch.set_num_threads(2)
def infer(rgb):
image = Image.fromarray(rgb).crop((100, 0, 700, 600))
tensor = processor(images=image, return_tensors="pt")["pixel_values"].cuda()
with torch.inference_mode():
logits = model(tensor).logits
logits = torch.nn.functional.interpolate(
logits, size=(512, 512), mode="bilinear", align_corners=False
)
confidence, labels = logits.softmax(1).max(1)
labels = labels[0].cpu().numpy().astype(np.uint8)
candidate = np.isin(labels, SURFACES) & (confidence[0].cpu().numpy() >= 0.55)
return labels.tobytes() + candidate.astype(np.uint8).tobytes()
infer(np.zeros((600, 800, 3), dtype=np.uint8))
class Handler(BaseHTTPRequestHandler):
def setup(self):
super().setup()
self.connection.settimeout(10)
def reply(self, status, body):
self.send_response(status)
self.send_header("Content-Type", "application/octet-stream")
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" else 404,
json.dumps({"model": "segformer-b2-ade150"}).encode(),
)
def do_POST(self):
if self.path != "/infer" or self.headers.get("Content-Length") != "1440000":
self.reply(400, b"Expected 800x600 raw RGB uint8")
return
try:
raw = self.rfile.read(1440000)
if len(raw) != 1440000:
raise ValueError("Incomplete camera frame")
self.reply(200, infer(np.frombuffer(raw, np.uint8).reshape(600, 800, 3)))
except (TimeoutError, ValueError, RuntimeError):
self.reply(500, b"Surface inference failed")
def log_message(self, *_):
pass
HTTPServer(("0.0.0.0", 8010), Handler).serve_forever()
if __name__ == "__main__":
main()
+54
View File
@@ -0,0 +1,54 @@
"""Scene-authoring admission for a full rover footprint, never an AI map.
Ground-height samples can miss a narrow trunk between sample rays. Test mesh
triangles against the occupied prism using the separating-axis theorem, which
also catches a face crossing the body when all of its vertices lie outside.
"""
import math
import numpy as np
def obstructing_triangles(vertices, faces, xy, heading, plane, *, step=0.1, height=1.1):
"""Count triangles above qualified step height inside the 1 x 1 m start.
``plane`` is z = a*(x-xy[0]) + b*(y-xy[1]) + c, fitted to the start's support.
This is a conservative preparation gate, not a claim of route traversability.
It neither changes the collider nor supplies privileged geometry to inference.
"""
triangles = np.asarray(vertices)[faces]
center_xy = np.asarray(xy)
selected = (triangles[:, :, :2].min(axis=1) <= center_xy + 0.71).all(axis=1) & (
triangles[:, :, :2].max(axis=1) >= center_xy - 0.71
).all(axis=1)
triangles = triangles[selected].astype(np.float64)
if not len(triangles):
return 0
delta = triangles[:, :, :2] - center_xy
angle = math.radians(heading)
rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
triangles[:, :, 2] -= delta @ np.asarray(plane[:2]) + plane[2]
triangles[:, :, :2] = delta @ rotation
low = step + 1e-4 # Same centimetre-scale capability boundary as navigation.
half = np.array([0.5, 0.5, (height - low) / 2])
triangles[:, :, 2] -= (height + low) / 2
selected = (triangles.min(axis=1) <= half).all(axis=1) & (triangles.max(axis=1) >= -half).all(
axis=1
)
triangles = triangles[selected]
if not len(triangles):
return 0
edges = np.roll(triangles, -1, axis=1) - triangles
axes = [np.cross(edges[:, 0], edges[:, 1])]
for edge in range(3):
for box_axis in np.eye(3):
axes.append(np.cross(edges[:, edge], box_axis))
overlaps = np.ones(len(triangles), dtype=bool)
for axis in axes:
projections = np.einsum("nvi,ni->nv", triangles, axis)
radius = np.abs(axis) @ half
overlaps &= (projections.min(axis=1) <= radius + 1e-10) & (
projections.max(axis=1) >= -radius - 1e-10
)
return int(overlaps.sum())
+101
View File
@@ -0,0 +1,101 @@
{
"name": "ndc-ai-polygon-splat-tools",
"version": "1.0.0",
"lockfileVersion": 3,
"requires": true,
"packages": {
"": {
"name": "ndc-ai-polygon-splat-tools",
"version": "1.0.0",
"dependencies": {
"@playcanvas/splat-transform": "3.4.0"
}
},
"node_modules/@adobe/spz": {
"version": "0.2.3",
"resolved": "https://registry.npmjs.org/@adobe/spz/-/spz-0.2.3.tgz",
"integrity": "sha512-iMiIB+FUxQ2vaNxP012fdIVd4yugyq0PUXBaknGfYDE3xpTNU5PrhbLtnDTGFE9anKOwjneOuanSKqaaNpC68Q==",
"license": "ISC"
},
"node_modules/@playcanvas/splat-transform": {
"version": "3.4.0",
"resolved": "https://registry.npmjs.org/@playcanvas/splat-transform/-/splat-transform-3.4.0.tgz",
"integrity": "sha512-I/lZFfHNzonye1QIne/2nF+wDbMx0xHK4k1IpzJmD3TfNc8ftmuVsWWKszYPkScWDG1vq2n9QjMXS9DARUPmcQ==",
"license": "MIT",
"dependencies": {
"@adobe/spz": "0.2.3",
"webgpu": "0.6.0"
},
"bin": {
"splat-transform": "bin/cli.mjs"
},
"engines": {
"node": ">=22.0.0"
},
"peerDependencies": {
"playcanvas": "^2.0.0"
}
},
"node_modules/@types/webxr": {
"version": "0.5.24",
"resolved": "https://registry.npmjs.org/@types/webxr/-/webxr-0.5.24.tgz",
"integrity": "sha512-h8fgEd/DpoS9CBrjEQXR+dIDraopAEfu4wYVNY2tEPwk60stPWhvZMf4Foo5FakuQ7HFZoa8WceaWFervK2Ovg==",
"license": "MIT",
"peer": true
},
"node_modules/@webgpu/types": {
"version": "0.1.74",
"resolved": "https://registry.npmjs.org/@webgpu/types/-/types-0.1.74.tgz",
"integrity": "sha512-lgiI4hbuLcI9unnm2cL/tvCaQU45dp0xcLWJh5uB/9MBGvIA4F8XIk7nSiBeg+K4xWGYwgZKn80LmERI5CxoTA==",
"license": "BSD-3-Clause"
},
"node_modules/debug": {
"version": "4.4.3",
"resolved": "https://registry.npmjs.org/debug/-/debug-4.4.3.tgz",
"integrity": "sha512-RGwwWnwQvkVfavKVt22FGLw+xYSdzARwm0ru6DhTVA3umU5hZc28V3kO4stgYryrTlLpuvgI9GiijltAjNbcqA==",
"license": "MIT",
"dependencies": {
"ms": "^2.1.3"
},
"engines": {
"node": ">=6.0"
},
"peerDependenciesMeta": {
"supports-color": {
"optional": true
}
}
},
"node_modules/ms": {
"version": "2.1.3",
"resolved": "https://registry.npmjs.org/ms/-/ms-2.1.3.tgz",
"integrity": "sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==",
"license": "MIT"
},
"node_modules/playcanvas": {
"version": "2.22.3",
"resolved": "https://registry.npmjs.org/playcanvas/-/playcanvas-2.22.3.tgz",
"integrity": "sha512-HjBbJmUqwqYk678nHu/hUniep0YgziAJ9oqwi13ye/rlDjIN2/bbmnCZexKa5IZDINSMkjqfhq+jsJhPL4wBNA==",
"license": "MIT",
"peer": true,
"dependencies": {
"@types/webxr": "^0.5.24",
"@webgpu/types": "^0.1.70"
},
"engines": {
"node": ">=18.3.0"
}
},
"node_modules/webgpu": {
"version": "0.6.0",
"resolved": "https://registry.npmjs.org/webgpu/-/webgpu-0.6.0.tgz",
"integrity": "sha512-z44ZU/+ypPELgQrv+um3/5Eqcq7gOtVZAmaRfwujJMwGMRSa6+CmjIAvEWw7O68Y95cnEsywhbYlcChQTX+IQg==",
"hasInstallScript": true,
"license": "MIT",
"dependencies": {
"@webgpu/types": "^0.1.72",
"debug": "^4.4.0"
}
}
}
}
@@ -0,0 +1,6 @@
{
"name": "ndc-ai-polygon-splat-tools",
"version": "1.0.0",
"private": true,
"dependencies": { "@playcanvas/splat-transform": "3.4.0" }
}
+44
View File
@@ -0,0 +1,44 @@
"""Start the prepared Worker and its private tunnel in one foreground SSH session."""
import argparse
import base64
import os
import re
parser = argparse.ArgumentParser()
parser.add_argument("--release", required=True, help="16-character build_bundle identity prefix")
parser.add_argument("--once", action="store_true", help="Exit after one episode")
args = parser.parse_args()
if not re.fullmatch(r"[a-f0-9]{16}", args.release):
parser.error("release must be a 16-character lowercase hexadecimal bundle identity")
script = r"""
$ErrorActionPreference='Stop'
$ProgressPreference='SilentlyContinue'
[Console]::OutputEncoding=[System.Text.Encoding]::UTF8
$b='D:\NDC_MISSIONCORE\runtime\simulation'
$worker="$b\releases\ai-polygon-RELEASE\simulation\ai-polygon\worker.py"
& "$b\isaac-sim-6.1.0\kit\python\python.exe" -u $worker `
--token-file "$b\private\worker.token" --state "$b\state" `
--isaac "$b\isaac-sim-6.1.0" ONCE
exit $LASTEXITCODE
""".replace("RELEASE", args.release).replace("ONCE", "--once" if args.once else "")
encoded = base64.b64encode(script.encode("utf-16-le")).decode()
os.execvp(
"ssh",
[
"ssh",
"-T",
"-o",
"BatchMode=yes",
"-o",
"ExitOnForwardFailure=yes",
"-o",
"ServerAliveInterval=5",
"-o",
"ServerAliveCountMax=3",
"-R",
"127.0.0.1:18081:127.0.0.1:8000",
"mission-gpu",
"powershell.exe -NoProfile -NonInteractive -EncodedCommand " + encoded,
],
)
+202
View File
@@ -0,0 +1,202 @@
"""Metric collision proxy and occlusion-aware virtual range sensor on Worker."""
import hashlib
import json
import math
import struct
from pathlib import Path
import numpy as np
def load_glb(path):
data = Path(path).read_bytes()
magic, version, size = struct.unpack_from("<III", data)
if (magic, version, size) != (0x46546C67, 2, len(data)):
raise ValueError("Expected a complete glTF 2 binary collision mesh")
offset, document, binary = 12, None, None
while offset < len(data):
length, kind = struct.unpack_from("<II", data, offset)
chunk = data[offset + 8 : offset + 8 + length]
if kind == 0x4E4F534A:
document = json.loads(chunk)
elif kind == 0x004E4942:
binary = chunk
offset += 8 + length
if document is None or binary is None:
raise ValueError("Missing mesh buffers")
def accessor(index):
spec = document["accessors"][index]
view = document["bufferViews"][spec["bufferView"]]
if "sparse" in spec or view.get("buffer", 0) != 0:
raise ValueError("Unsupported sparse/external collision buffer")
dtype = {5123: "<u2", 5125: "<u4", 5126: "<f4"}[spec["componentType"]]
width = {"SCALAR": 1, "VEC3": 3}[spec["type"]]
item = np.dtype(dtype).itemsize
start = view.get("byteOffset", 0) + spec.get("byteOffset", 0)
return np.ndarray(
(spec["count"], width),
dtype=dtype,
buffer=binary,
offset=start,
strides=(view.get("byteStride", width * item), item),
).copy()
vertices, faces, base = [], [], 0
# SplatTransform collision exports bake coordinates; reject transforms so
# we never silently misregister contact geometry against the Gaussian view.
for node in document.get("nodes", []):
if any(k in node for k in ("matrix", "translation", "rotation", "scale")):
raise ValueError("Collision node transform must be baked")
for mesh in document["meshes"]:
for primitive in mesh["primitives"]:
if primitive.get("mode", 4) != 4:
raise ValueError("Collision mesh must contain triangles")
points = accessor(primitive["attributes"]["POSITION"])
triangles = accessor(primitive["indices"]).reshape(-1, 3).astype(np.int32)
if not np.isfinite(points).all() or triangles.max() >= len(points):
raise ValueError("Invalid collision vertices")
vertices.append(points[:, [0, 2, 1]] * np.array([1, -1, 1], np.float32))
faces.append(triangles + base)
base += len(points)
return np.concatenate(vertices), np.concatenate(faces)
def ground_intersections(vertices, faces, x, y):
"""Vertical mesh intersections, used only to place the initial rigid body."""
triangles = vertices[faces]
low, high = triangles[:, :, :2].min(axis=1), triangles[:, :, :2].max(axis=1)
candidate = triangles[
(low[:, 0] <= x) & (high[:, 0] >= x) & (low[:, 1] <= y) & (high[:, 1] >= y)
]
if not len(candidate):
return np.empty(0)
a, b, c = candidate[:, 0], candidate[:, 1], candidate[:, 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]
def install_terrain(stage, manifest_path, world):
from pxr import UsdGeom, UsdPhysics, UsdShade
from rover_profile import PROFILE
from k1link.simulation.ai_polygon.terrain_contract import terrain_matches
manifest = json.loads(Path(manifest_path).read_text(encoding="utf-8-sig"))
if not terrain_matches(manifest, world):
raise ValueError("Terrain does not match the world calibration")
path = Path(manifest["collider"])
if hashlib.sha256(path.read_bytes()).hexdigest() != manifest["collider_sha256"]:
raise ValueError("Collision mesh identity changed")
points, triangles = load_glb(path)
settings = world["settings"]
heights = ground_intersections(points, triangles, *settings["spawn_xy"])
candidates = heights[np.abs(heights - settings["ground_z"]) < 0.8]
if not len(candidates):
raise ValueError("No reconstructed ground at the configured spawn")
# Place the whole footprint above nearby ground, without embedding a wheel
# in a stone beside the centre ray. This is setup, never a planner input.
angle = math.radians(settings["heading_degrees"])
support, offsets = [], []
for dx in (-0.5, 0, 0.5):
for dy in (-0.5, 0, 0.5):
x = settings["spawn_xy"][0] + dx * math.cos(angle) - dy * math.sin(angle)
y = settings["spawn_xy"][1] + dx * math.sin(angle) + dy * math.cos(angle)
intersections = ground_intersections(points, triangles, x, y)
nearby = intersections[np.abs(intersections - settings["ground_z"]) < 0.8]
if not len(nearby):
raise ValueError("Rover footprint has no reconstructed support")
support.append(float(nearby.max()))
offsets.append([x - settings["spawn_xy"][0], y - settings["spawn_xy"][1], 1])
plane = np.linalg.lstsq(np.asarray(offsets), np.asarray(support), rcond=None)[0]
residual = np.asarray(support) - np.asarray(offsets) @ plane
normal = np.array([-plane[0], -plane[1], 1.0])
normal /= np.linalg.norm(normal)
if np.max(np.abs(residual)) > 0.12 or normal[2] < math.cos(math.radians(25)):
raise ValueError("Configured spawn is too uneven for the metre-wide rover")
from spawn_clearance import obstructing_triangles
if obstructing_triangles(
points,
triangles,
settings["spawn_xy"],
settings["heading_degrees"],
plane,
step=PROFILE["max_step_m"],
):
raise ValueError("Configured spawn contains an obstacle inside the rover footprint")
# Align to the local support plane before gravity settles the suspensionless
# lab chassis. Wheel centres start 4 cm above the highest residual contact.
wheel_center_z = float(
plane[2] + (PROFILE["wheel_radius_m"] + 0.04) / normal[2] + max(0, residual.max())
)
mesh = UsdGeom.Mesh.Define(stage, "/World/Terrain")
mesh.CreatePointsAttr(points.tolist())
mesh.CreateFaceVertexCountsAttr([3] * len(triangles))
mesh.CreateFaceVertexIndicesAttr(triangles.reshape(-1).tolist())
mesh.CreateSubdivisionSchemeAttr(UsdGeom.Tokens.none)
mesh.CreateDoubleSidedAttr(True)
mesh.CreateVisibilityAttr(UsdGeom.Tokens.invisible)
UsdPhysics.CollisionAPI.Apply(mesh.GetPrim())
UsdPhysics.MeshCollisionAPI.Apply(mesh.GetPrim()).CreateApproximationAttr("none")
material = UsdShade.Material.Define(stage, "/World/Materials/Terrain")
surface = UsdPhysics.MaterialAPI.Apply(material.GetPrim())
surface.CreateStaticFrictionAttr(0.9)
surface.CreateDynamicFrictionAttr(0.8)
surface.CreateRestitutionAttr(0)
UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(
material, UsdShade.Tokens.weakerThanDescendants, "physics"
)
return wheel_center_z, dict(
manifest,
vertex_count=len(points),
triangle_count=len(triangles),
initial_contact_z=float(plane[2]),
initial_ground_normal=normal.tolist(),
support_residual_m=float(np.max(np.abs(residual))),
)
class RangeSensor:
"""Front 360-degree LiDAR plus rear near-field fan; first physical hit.
Separate mounts observe the ground beyond each bumper without seeing
through the chassis. The rear fan adds 660 rays to the 2160 front rays.
"""
def __init__(self):
import omni.physx
self.query = omni.physx.get_physx_scene_query_interface()
elevations = (-80, -75, -70, -60, -45, -35, -28, -22, -18, -14, -10, -5, 0, 5, 15)
self.directions = [
(math.cos(e) * math.cos(a), math.cos(e) * math.sin(a), math.sin(e))
for e in np.radians(elevations)
for a in np.radians(np.arange(-180, 180, 2.5))
]
self.rear_directions = [
(math.cos(e) * math.cos(a), math.cos(e) * math.sin(a), math.sin(e))
for e in np.radians(np.arange(-80, 16, 5))
for a in np.radians(np.arange(100, 261, 5))
]
def capture(self, origin, rotation, rear_origin=None):
from pxr import Gf
points = []
mounts = [(origin, self.directions)]
if rear_origin is not None:
mounts.append((rear_origin, self.rear_directions))
for mount, directions in mounts:
for direction in directions:
ray = rotation.TransformDir(Gf.Vec3d(*direction)).GetNormalized()
hit = self.query.raycast_closest(tuple(mount), tuple(ray), 8.0, bothSides=True)
if hit["hit"] and not str(hit.get("rigidBody", "")).startswith("/World/Rover"):
points.append(hit["position"])
return np.asarray(points, dtype=np.float32).reshape(-1, 3)
+212
View File
@@ -0,0 +1,212 @@
"""Single-owner Windows coordinator; no GPU work while idle.
Core owns admission. Each episode gets a fresh native Isaac process and the
additive model stack. A missing Core heartbeat terminates this episode; it never
resumes an old command. A leftover active.json requires explicit reconciliation.
"""
import argparse
import hashlib
import json
import os
import re
import subprocess
import threading
import time
from pathlib import Path
from uuid import uuid4
from core_client import CoreClient
from model_stack import ROOT, ModelStack, sha256
def terminate_episode(child):
"""A concurrent normal exit is success; only a still-running child is failure."""
if child.poll() is None:
subprocess.run(
["taskkill", "/PID", str(child.pid), "/T", "/F"],
capture_output=True,
check=False,
timeout=30,
)
child.wait(timeout=30)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--core", default="http://127.0.0.1:18081")
parser.add_argument("--token-file", type=Path, required=True)
parser.add_argument("--state", type=Path, required=True)
parser.add_argument("--isaac", type=Path, required=True)
parser.add_argument("--once", action="store_true")
args = parser.parse_args()
args.state.mkdir(parents=True, exist_ok=True)
active_file = args.state / "active.json"
if active_file.exists():
raise RuntimeError("Reconcile the previous simulation before starting a new worker")
lock = args.state / "worker.lock"
instance = uuid4().hex
client = CoreClient(args.core, args.token_file, instance)
stack = ModelStack()
stack.preflight()
robot_root = args.isaac.parent / "assets/jetbot-6.1-v1"
robot_manifest = robot_root / "asset-manifest.json"
assets = json.loads(robot_manifest.read_text(encoding="utf-8-sig"))
for asset in assets["files"]:
if sha256(robot_root / asset["path"]) != asset["sha256"]:
raise RuntimeError("Prepared Jetbot asset changed")
sources = {
"worker": sha256(ROOT / "worker.py"),
"scene": sha256(ROOT / "run_scene.py"),
"models": sha256(stack.profile_path),
"robot": sha256(robot_manifest),
}
hello = {
"worker_id": "worker-006-ai-polygon",
"instance_id": instance,
"runtime": "isaac-sim-6.1",
"model_ids": [m["id"] for m in stack.profile["models"]],
"profile_sha256": hashlib.sha256(json.dumps(sources, sort_keys=True).encode()).hexdigest(),
"runtime_sources": sources,
}
lost = threading.Event()
finished = threading.Event()
child = None
def heartbeat():
while not finished.wait(3):
try:
client.request("/worker/heartbeat", {"instance_id": instance})
except Exception:
lost.set()
return
fd = os.open(lock, os.O_WRONLY | os.O_CREAT | os.O_EXCL)
os.close(fd)
try:
client.request("/worker/register", hello)
threading.Thread(target=heartbeat, daemon=True).start()
while not lost.is_set():
polled = client.request("/worker/poll", {"instance_id": instance})
if polled["action"] == "idle":
time.sleep(0.5)
continue
run = polled["run"]
if not re.fullmatch(r"airun-[a-f0-9]{32}", run["run_id"]):
raise ValueError("Invalid run identity")
episode = args.state / run["run_id"]
episode.mkdir()
run_file = episode / "run.json"
run_file.write_text(json.dumps(run))
active_file.write_text(
json.dumps(
{
"run_id": run["run_id"],
"instance_id": instance,
"profile_sha256": hello["profile_sha256"],
}
)
)
result = {"outcome": "failed", "message": "Worker прервал прогон."}
released = False
try:
cache = args.state / "worlds"
cache.mkdir(exist_ok=True)
source = cache / (run["world"]["sha256"] + ".ply")
client.download(run["world"], source)
if lost.is_set():
raise RuntimeError("Core connection was lost during scene preparation")
control = client.request(
"/worker/runs/" + run["run_id"] + "/progress", {"phase": "models"}
)
if control["control"] == "stop":
raise InterruptedError("Stopped before model startup")
stack.start(
cancelled=lambda run_id=run["run_id"]: (
lost.is_set()
or client.request("/runs/" + run_id)["control"] == "stop"
)
)
if lost.is_set():
raise RuntimeError("Core connection was lost during model startup")
control = client.request(
"/worker/runs/" + run["run_id"] + "/progress", {"phase": "scene"}
)
if control["control"] == "stop":
raise InterruptedError("Stopped before scene startup")
command = [
str(args.isaac / "python.bat"),
str(ROOT / "run_scene.py"),
"--run",
str(run_file),
"--source",
str(source),
"--core",
args.core,
"--token-file",
str(args.token_file),
"--instance",
instance,
]
with (episode / "isaac.log").open("wb") as output:
child = subprocess.Popen(command, stdout=output, stderr=subprocess.STDOUT)
stopped = False
next_control_check = 0.0
deadline = time.monotonic() + 300 + run["request"]["max_steps"] * 15
while (
child.poll() is None and not lost.is_set() and time.monotonic() < deadline
):
if time.monotonic() >= next_control_check:
state = client.request("/runs/" + run["run_id"])
if state["control"] == "stop" or state["state"] == "failed":
stopped = state["control"] == "stop"
break
next_control_check = time.monotonic() + 1
time.sleep(0.2)
if child.poll() is None:
terminate_episode(child)
child.wait(timeout=30)
child = None
result_file = episode / "result.json"
if stopped:
result = {"outcome": "stopped", "message": "Прогон остановлен."}
elif result_file.exists() and not lost.is_set():
result = json.loads(result_file.read_text())
except Exception as exc:
(episode / "worker-error.txt").write_text(type(exc).__name__ + ": " + str(exc))
finally:
if child is not None and child.poll() is None:
terminate_episode(child)
child.wait(timeout=30)
stack.stop()
released = True
# A Stop may race with the final sample/normal native exit. It remains a
# successful cancellation only after native and model cleanup above.
if not lost.is_set():
state = client.request("/runs/" + run["run_id"])
if state["control"] == "stop" and state["state"] == "stopping":
result = {"outcome": "stopped", "message": "Движение и inference остановлены."}
# Do not release the reservation until native process AND GPU containers are gone.
for attempt in range(10):
try:
if lost.is_set():
client.request("/worker/register", hello)
client.request(
"/worker/runs/" + run["run_id"] + "/finish",
{**result, "instance_id": instance, "resources_released": released},
)
active_file.unlink()
break
except Exception:
if attempt == 9:
raise
time.sleep(1)
if args.once or lost.is_set():
break
finally:
finished.set()
lock.unlink(missing_ok=True)
if __name__ == "__main__":
main()
+9
View File
@@ -1,5 +1,6 @@
from __future__ import annotations from __future__ import annotations
import hashlib
import json import json
import os import os
import tempfile import tempfile
@@ -8,6 +9,14 @@ from pathlib import Path
from typing import Any from typing import Any
def canonical_json_sha256(value: object) -> str:
"""Platform-independent identity for shared Core/Worker JSON contracts."""
payload = json.dumps(
value, ensure_ascii=False, allow_nan=False, separators=(",", ":"), sort_keys=True
).encode("utf-8")
return hashlib.sha256(payload).hexdigest()
def utc_now_iso() -> str: def utc_now_iso() -> str:
"""Return a stable UTC timestamp for manifests and capture artifacts.""" """Return a stable UTC timestamp for manifests and capture artifacts."""
return datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z") return datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z")
+75 -29
View File
@@ -1,33 +1,41 @@
"""Observation-only laboratory orchestration contracts.""" """Observation contracts; platform-specific services are loaded only on demand.
from k1link.observatory.canonical_result import ( Importing a portable composition on Windows must not import Core's POSIX
is_admitted_observatory_recorded_result, artifact gateway or eagerly initialize session/recording infrastructure.
) """
from k1link.observatory.run_preparations import (
MAX_RUN_PREPARATION_RECORDS, from importlib import import_module
MAX_RUN_PREPARATION_STORAGE_BYTES, from typing import TYPE_CHECKING
OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA,
OBSERVATORY_RUN_PREPARATION_SCHEMA, if TYPE_CHECKING:
RUN_PREPARATION_DATABASE_NAME, from k1link.observatory.canonical_result import (
RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS, is_admitted_observatory_recorded_result,
ObservatoryRunPreparation, )
ObservatoryRunPreparationCapacityError, from k1link.observatory.run_preparations import (
ObservatoryRunPreparationConflictError, MAX_RUN_PREPARATION_RECORDS,
ObservatoryRunPreparationError, MAX_RUN_PREPARATION_STORAGE_BYTES,
ObservatoryRunPreparationIntegrityError, OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA,
ObservatoryRunPreparationIntent, OBSERVATORY_RUN_PREPARATION_SCHEMA,
ObservatoryRunPreparationLedger, RUN_PREPARATION_DATABASE_NAME,
ObservatoryRunPreparationNotFoundError, RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS,
load_observatory_run_preparation_ledger, ObservatoryRunPreparation,
observatory_run_preparation_request_sha256, ObservatoryRunPreparationCapacityError,
) ObservatoryRunPreparationConflictError,
from k1link.observatory.setups import ( ObservatoryRunPreparationError,
LABORATORY_SETUP_CATALOG_SCHEMA, ObservatoryRunPreparationIntegrityError,
LABORATORY_SETUP_REGISTRY_SCHEMA, ObservatoryRunPreparationIntent,
OBSERVATORY_CALCULATION_PROFILE_SCHEMA, ObservatoryRunPreparationLedger,
LaboratorySetupRegistry, ObservatoryRunPreparationNotFoundError,
LaboratorySetupRegistryError, load_observatory_run_preparation_ledger,
) observatory_run_preparation_request_sha256,
)
from k1link.observatory.setups import (
LABORATORY_SETUP_CATALOG_SCHEMA,
LABORATORY_SETUP_REGISTRY_SCHEMA,
OBSERVATORY_CALCULATION_PROFILE_SCHEMA,
LaboratorySetupRegistry,
LaboratorySetupRegistryError,
)
__all__ = [ __all__ = [
"LABORATORY_SETUP_CATALOG_SCHEMA", "LABORATORY_SETUP_CATALOG_SCHEMA",
@@ -53,3 +61,41 @@ __all__ = [
"load_observatory_run_preparation_ledger", "load_observatory_run_preparation_ledger",
"observatory_run_preparation_request_sha256", "observatory_run_preparation_request_sha256",
] ]
_EXPORTS = {
"is_admitted_observatory_recorded_result": "k1link.observatory.canonical_result",
"MAX_RUN_PREPARATION_RECORDS": "k1link.observatory.run_preparations",
"MAX_RUN_PREPARATION_STORAGE_BYTES": "k1link.observatory.run_preparations",
"OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA": "k1link.observatory.run_preparations",
"OBSERVATORY_RUN_PREPARATION_SCHEMA": "k1link.observatory.run_preparations",
"RUN_PREPARATION_DATABASE_NAME": "k1link.observatory.run_preparations",
"RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS": "k1link.observatory.run_preparations",
"ObservatoryRunPreparation": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationCapacityError": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationConflictError": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationError": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationIntegrityError": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationIntent": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationLedger": "k1link.observatory.run_preparations",
"ObservatoryRunPreparationNotFoundError": "k1link.observatory.run_preparations",
"load_observatory_run_preparation_ledger": "k1link.observatory.run_preparations",
"observatory_run_preparation_request_sha256": "k1link.observatory.run_preparations",
"LABORATORY_SETUP_CATALOG_SCHEMA": "k1link.observatory.setups",
"LABORATORY_SETUP_REGISTRY_SCHEMA": "k1link.observatory.setups",
"OBSERVATORY_CALCULATION_PROFILE_SCHEMA": "k1link.observatory.setups",
"LaboratorySetupRegistry": "k1link.observatory.setups",
"LaboratorySetupRegistryError": "k1link.observatory.setups",
}
def __getattr__(name: str):
module = _EXPORTS.get(name)
if module is None:
raise AttributeError(f"module {__name__!r} has no attribute {name!r}")
value = getattr(import_module(module), name)
globals()[name] = value
return value
def __dir__():
return sorted(set(globals()) | set(__all__))
+14 -4
View File
@@ -12,7 +12,7 @@ from dataclasses import dataclass
from pathlib import Path from pathlib import Path
from typing import Final, cast from typing import Final, cast
from k1link.observatory.portable_run_definitions import canonical_sha256 from k1link.artifacts import canonical_json_sha256 as canonical_sha256
COMPOSITION_SCHEMA: Final = "missioncore.observatory-ai-composition/v1" COMPOSITION_SCHEMA: Final = "missioncore.observatory-ai-composition/v1"
MODULE_SCHEMA: Final = "missioncore.observatory-ai-module/v1" MODULE_SCHEMA: Final = "missioncore.observatory-ai-module/v1"
@@ -179,6 +179,11 @@ class CompositionSpec:
"""Source-independent graph, in deterministic topological execution order.""" """Source-independent graph, in deterministic topological execution order."""
nodes: tuple[CompositionNode, ...] nodes: tuple[CompositionNode, ...]
execution_mode: str = "recorded-observation-only"
def __post_init__(self) -> None:
if self.execution_mode not in ("recorded-observation-only", "worker-local-simulation"):
raise CompositionError("unsupported composition execution mode")
@property @property
def source_capabilities(self) -> tuple[str, ...]: def source_capabilities(self) -> tuple[str, ...]:
@@ -212,7 +217,10 @@ class CompositionSpec:
"nodes": [node.as_dict() for node in self.nodes], "nodes": [node.as_dict() for node in self.nodes],
"source_capabilities": list(self.source_capabilities), "source_capabilities": list(self.source_capabilities),
"outputs": list(self.outputs), "outputs": list(self.outputs),
"execution": {"max_parallel_nodes": 1, "mode": "recorded-observation-only"}, "execution": {
"max_parallel_nodes": 2 if self.execution_mode == "worker-local-simulation" else 1,
"mode": self.execution_mode,
},
} }
@property @property
@@ -286,7 +294,9 @@ class ModuleRegistry:
], ],
} }
def compose(self, document: object) -> CompositionSpec: def compose(
self, document: object, *, execution_mode: str = "recorded-observation-only"
) -> CompositionSpec:
root = _object(document, {"schema_version", "selections"}) root = _object(document, {"schema_version", "selections"})
if root["schema_version"] != COMPOSITION_SCHEMA: if root["schema_version"] != COMPOSITION_SCHEMA:
raise CompositionError("unsupported composition schema") raise CompositionError("unsupported composition schema")
@@ -381,7 +391,7 @@ class ModuleRegistry:
for key in ready: for key in ready:
ordered.append(pending.pop(key)) ordered.append(pending.pop(key))
emitted.add(key) emitted.add(key)
return CompositionSpec(tuple(ordered)) return CompositionSpec(tuple(ordered), execution_mode=execution_mode)
def node_input_identity( def node_input_identity(
+53 -1
View File
@@ -97,6 +97,11 @@ _AUTHORITY: Final = {
} }
_SCHEMA_SQL = """ _SCHEMA_SQL = """
CREATE TABLE IF NOT EXISTS simulation_worker_reservation (
singleton INTEGER PRIMARY KEY CHECK (singleton = 1),
owner_id TEXT NOT NULL,
created_at_utc TEXT NOT NULL
);
CREATE TABLE IF NOT EXISTS observatory_recorded_jobs ( CREATE TABLE IF NOT EXISTS observatory_recorded_jobs (
job_id TEXT PRIMARY KEY, job_id TEXT PRIMARY KEY,
idempotency_key TEXT NOT NULL UNIQUE, idempotency_key TEXT NOT NULL UNIQUE,
@@ -1185,6 +1190,46 @@ class ObservatoryRecordedJobQueue:
self._lock = threading.RLock() self._lock = threading.RLock()
self._initialize() self._initialize()
def reserve_simulation(self, owner_id: str) -> None:
"""Reserve this Worker's GPU atomically against recorded/live admission.
A lost heartbeat does not prove GPU release. This reservation survives
restarts and is removed only by the exact simulator's release receipt.
"""
if not re.fullmatch(r"airun-[a-f0-9]{32}", owner_id):
raise ValueError("invalid simulation resource owner")
with self._transaction() as connection:
existing = connection.execute(
"SELECT owner_id FROM simulation_worker_reservation"
).fetchone()
if existing is not None:
if existing["owner_id"] == owner_id:
return
raise ObservatoryRecordedQueueBusyError("Worker занят другим прогоном симуляции.")
busy = connection.execute(
"SELECT job_id FROM observatory_recorded_jobs "
"WHERE state IN ('claimed', 'running', 'paused', 'preemption-pending', "
"'reconciliation-required') LIMIT 1"
).fetchone()
if busy is not None or self._open_live_lease_row(connection) is not None:
raise ObservatoryRecordedQueueBusyError("Worker занят задачей AI Inference.")
connection.execute(
"INSERT INTO simulation_worker_reservation VALUES (1, ?, ?)",
(owner_id, self._timestamp()),
)
def release_simulation(self, owner_id: str) -> None:
"""Called only after the trusted simulator reports all GPU work stopped."""
with self._transaction() as connection:
existing = connection.execute(
"SELECT owner_id FROM simulation_worker_reservation"
).fetchone()
if existing is not None and existing["owner_id"] != owner_id:
raise ObservatoryRecordedQueueConflictError("simulation resource owner changed")
connection.execute(
"DELETE FROM simulation_worker_reservation WHERE owner_id = ?", (owner_id,)
)
def resolve_definition( def resolve_definition(
self, self,
setup_id: str, setup_id: str,
@@ -1444,7 +1489,10 @@ class ObservatoryRecordedJobQueue:
label="legacy claim receipt", label="legacy claim receipt",
) )
row = None row = None
if self._open_live_lease_row(connection) is None: simulation = connection.execute(
"SELECT owner_id FROM simulation_worker_reservation"
).fetchone()
if self._open_live_lease_row(connection) is None and simulation is None:
active_owner = connection.execute( active_owner = connection.execute(
"SELECT job_id FROM observatory_recorded_jobs " "SELECT job_id FROM observatory_recorded_jobs "
"WHERE state IN ('claimed', 'running', 'preemption-pending', " "WHERE state IN ('claimed', 'running', 'preemption-pending', "
@@ -2382,6 +2430,10 @@ class ObservatoryRecordedJobQueue:
_validate_pattern(lease_id, _LEASE_ID, "live lease id") _validate_pattern(lease_id, _LEASE_ID, "live lease id")
self.recover_stale_claims() self.recover_stale_claims()
with self._transaction() as connection: with self._transaction() as connection:
if connection.execute("SELECT owner_id FROM simulation_worker_reservation").fetchone():
raise ObservatoryRecordedQueueBusyError(
"simulation has not released the Worker GPU"
)
lease = self._get_live_lease(connection, lease_id) lease = self._get_live_lease(connection, lease_id)
if lease.state == "active": if lease.state == "active":
return lease return lease
+155 -74
View File
@@ -1,78 +1,86 @@
"""Mission Core qualification and simulation boundaries.""" """Mission Core simulation contracts, with platform services loaded on demand.
from k1link.simulation.contracts import ( Portable AI compositions do not require the legacy S0/YAML or POSIX process
AckermannControlSetpoint, supervisor when loaded by the Windows Worker coordinator.
AuthorityProfile, """
CommandAuthorityScope,
ControlProfile, from importlib import import_module
ControlSetpoint, from typing import TYPE_CHECKING
DifferentialControlSetpoint,
ProviderPin, if TYPE_CHECKING:
QualificationArtifact, from k1link.simulation.contracts import (
QualificationEvent, AckermannControlSetpoint,
QualificationRun, AuthorityProfile,
ReproducibilityTier, CommandAuthorityScope,
RunKind, ControlProfile,
RunState, ControlSetpoint,
SimulationContractError, DifferentialControlSetpoint,
) ProviderPin,
from k1link.simulation.orchestrator import ( QualificationArtifact,
ActiveQualificationRunError, QualificationEvent,
SimulationApplicationService, QualificationRun,
SimulationOrchestratorError, ReproducibilityTier,
SimulationWorkerPort, RunKind,
WorkerStartResult, RunState,
WorkerStopResult, SimulationContractError,
) )
from k1link.simulation.process_supervisor import ( from k1link.simulation.orchestrator import (
OwnedProcess, ActiveQualificationRunError,
PosixProcessSupervisor, SimulationApplicationService,
ProcessSpec, SimulationOrchestratorError,
ProcessStopResult, SimulationWorkerPort,
ProcessSupervisorError, WorkerStartResult,
) WorkerStopResult,
from k1link.simulation.provider_contract import ( )
PROVIDER_PROFILE_SCHEMA, from k1link.simulation.process_supervisor import (
ProviderRole, OwnedProcess,
SimulationClockDescriptor, PosixProcessSupervisor,
SimulationProviderContractError, ProcessSpec,
SimulationProviderDescriptor, ProcessStopResult,
SimulationProviderProfile, ProcessSupervisorError,
) )
from k1link.simulation.run_store import ( from k1link.simulation.provider_contract import (
QualificationRunConflictError, PROVIDER_PROFILE_SCHEMA,
QualificationRunIntegrityError, ProviderRole,
QualificationRunNotFoundError, SimulationClockDescriptor,
QualificationRunStore, SimulationProviderContractError,
QualificationRunStoreError, SimulationProviderDescriptor,
QualificationRunTransitionError, SimulationProviderProfile,
) )
from k1link.simulation.s0 import ( from k1link.simulation.run_store import (
CheckStatus, QualificationRunConflictError,
DoctorVerdict, QualificationRunIntegrityError,
RuntimeAcceptance, QualificationRunNotFoundError,
S0DoctorReport, QualificationRunStore,
S0Profile, QualificationRunStoreError,
S0ProfileError, QualificationRunTransitionError,
load_s0_profile, )
run_s0_doctor, from k1link.simulation.s0 import (
) CheckStatus,
from k1link.simulation.stock_rover import ( DoctorVerdict,
LIFECYCLE_PROFILE_SCHEMA, RuntimeAcceptance,
StockRoverLifecycleProfile, S0DoctorReport,
StockRoverProfileError, S0Profile,
StockRoverTargetPaths, S0ProfileError,
load_stock_rover_lifecycle_profile, load_s0_profile,
stock_rover_process_environment, run_s0_doctor,
stock_rover_process_specs, )
) from k1link.simulation.stock_rover import (
from k1link.simulation.worker import ( LIFECYCLE_PROFILE_SCHEMA,
LocalProcessWorkerAdapter, StockRoverLifecycleProfile,
S0WorkerGuard, StockRoverProfileError,
SimulationWorldControl, StockRoverTargetPaths,
WorkerAdmission, load_stock_rover_lifecycle_profile,
WorkerAdmissionError, stock_rover_process_environment,
) stock_rover_process_specs,
)
from k1link.simulation.worker import (
LocalProcessWorkerAdapter,
S0WorkerGuard,
SimulationWorldControl,
WorkerAdmission,
WorkerAdmissionError,
)
__all__ = [ __all__ = [
"AckermannControlSetpoint", "AckermannControlSetpoint",
@@ -133,3 +141,76 @@ __all__ = [
"stock_rover_process_environment", "stock_rover_process_environment",
"stock_rover_process_specs", "stock_rover_process_specs",
] ]
_EXPORTS = {
"AckermannControlSetpoint": "k1link.simulation.contracts",
"AuthorityProfile": "k1link.simulation.contracts",
"CommandAuthorityScope": "k1link.simulation.contracts",
"ControlProfile": "k1link.simulation.contracts",
"ControlSetpoint": "k1link.simulation.contracts",
"DifferentialControlSetpoint": "k1link.simulation.contracts",
"ProviderPin": "k1link.simulation.contracts",
"QualificationArtifact": "k1link.simulation.contracts",
"QualificationEvent": "k1link.simulation.contracts",
"QualificationRun": "k1link.simulation.contracts",
"ReproducibilityTier": "k1link.simulation.contracts",
"RunKind": "k1link.simulation.contracts",
"RunState": "k1link.simulation.contracts",
"SimulationContractError": "k1link.simulation.contracts",
"ActiveQualificationRunError": "k1link.simulation.orchestrator",
"SimulationApplicationService": "k1link.simulation.orchestrator",
"SimulationOrchestratorError": "k1link.simulation.orchestrator",
"SimulationWorkerPort": "k1link.simulation.orchestrator",
"WorkerStartResult": "k1link.simulation.orchestrator",
"WorkerStopResult": "k1link.simulation.orchestrator",
"OwnedProcess": "k1link.simulation.process_supervisor",
"PosixProcessSupervisor": "k1link.simulation.process_supervisor",
"ProcessSpec": "k1link.simulation.process_supervisor",
"ProcessStopResult": "k1link.simulation.process_supervisor",
"ProcessSupervisorError": "k1link.simulation.process_supervisor",
"PROVIDER_PROFILE_SCHEMA": "k1link.simulation.provider_contract",
"ProviderRole": "k1link.simulation.provider_contract",
"SimulationClockDescriptor": "k1link.simulation.provider_contract",
"SimulationProviderContractError": "k1link.simulation.provider_contract",
"SimulationProviderDescriptor": "k1link.simulation.provider_contract",
"SimulationProviderProfile": "k1link.simulation.provider_contract",
"QualificationRunConflictError": "k1link.simulation.run_store",
"QualificationRunIntegrityError": "k1link.simulation.run_store",
"QualificationRunNotFoundError": "k1link.simulation.run_store",
"QualificationRunStore": "k1link.simulation.run_store",
"QualificationRunStoreError": "k1link.simulation.run_store",
"QualificationRunTransitionError": "k1link.simulation.run_store",
"CheckStatus": "k1link.simulation.s0",
"DoctorVerdict": "k1link.simulation.s0",
"RuntimeAcceptance": "k1link.simulation.s0",
"S0DoctorReport": "k1link.simulation.s0",
"S0Profile": "k1link.simulation.s0",
"S0ProfileError": "k1link.simulation.s0",
"load_s0_profile": "k1link.simulation.s0",
"run_s0_doctor": "k1link.simulation.s0",
"LIFECYCLE_PROFILE_SCHEMA": "k1link.simulation.stock_rover",
"StockRoverLifecycleProfile": "k1link.simulation.stock_rover",
"StockRoverProfileError": "k1link.simulation.stock_rover",
"StockRoverTargetPaths": "k1link.simulation.stock_rover",
"load_stock_rover_lifecycle_profile": "k1link.simulation.stock_rover",
"stock_rover_process_environment": "k1link.simulation.stock_rover",
"stock_rover_process_specs": "k1link.simulation.stock_rover",
"LocalProcessWorkerAdapter": "k1link.simulation.worker",
"S0WorkerGuard": "k1link.simulation.worker",
"SimulationWorldControl": "k1link.simulation.worker",
"WorkerAdmission": "k1link.simulation.worker",
"WorkerAdmissionError": "k1link.simulation.worker",
}
def __getattr__(name: str):
module = _EXPORTS.get(name)
if module is None:
raise AttributeError(f"module {__name__!r} has no attribute {name!r}")
value = getattr(import_module(module), name)
globals()[name] = value
return value
def __dir__():
return sorted(set(globals()) | set(__all__))
@@ -0,0 +1 @@
"""Independent, virtual-only AI polygon; never routes assets through the LCC pipeline."""
@@ -0,0 +1,194 @@
"""Simulation providers use the same immutable typed composition as AI Inference.
Pinhole camera contracts are intentionally distinct from recorded K1/KB4 ports.
Only installed adapters are executable; selection JSON never carries code.
"""
import hashlib
import json
from pathlib import Path
from k1link.observatory.modular_composition import (
COMPOSITION_SCHEMA,
CompositionError,
ModuleRegistry,
ModuleSpec,
canonical_bytes,
)
def digest(value):
return hashlib.sha256(canonical_bytes(value)).hexdigest()
def registry(adapter_root: Path) -> ModuleRegistry:
profile = json.loads((adapter_root / "models.worker-006.json").read_text())
models = {m["id"]: m for m in profile["models"]}
implementation = hashlib.sha256(
Path(__file__).with_name("inference.py").read_bytes()
).hexdigest()
ddr, detector = models["ddrnet-goose-pytorch-reference"], models["rf_detr_large"]
nav = profile["navigation"]
ade = models["segformer-b2-ade150"]
return ModuleRegistry(
(
ModuleSpec(
"simulation-ddrnet-goose",
"DDRNet · GOOSE 64",
"segmentation",
ddr["image"].removeprefix("sha256:"),
implementation,
ddr["checkpoint_sha256"],
digest(
{
"camera": profile["camera"],
"preprocess": ddr["preprocess"],
"labels": profile["labels_sha256"],
}
),
("source.camera.rgb",),
("segmentation.labels", "segmentation.surface"),
),
ModuleSpec(
"simulation-segformer-ade",
"SegFormer · природные поверхности",
"segmentation",
ade["image"].removeprefix("sha256:"),
digest(
{
"client": implementation,
"server": hashlib.sha256(
(adapter_root / "segformer/server.py").read_bytes()
).hexdigest(),
}
),
ade["checkpoint_sha256"],
digest(
{
"camera": profile["camera"],
"preprocess": ade["preprocess"],
"config": ade["config_sha256"],
"processor": ade["processor_sha256"],
"output": "ade150-labels-and-bool-surface-candidate-square512-v1",
}
),
("source.camera.rgb",),
("segmentation.labels", "segmentation.surface"),
),
ModuleSpec(
"simulation-rf-detr",
"RF-DETR · люди и животные",
"detection",
detector["image"].removeprefix("sha256:"),
implementation,
detector["checkpoint_sha256"],
digest(
{
"camera": profile["camera"],
"preprocess": detector["preprocess"],
"output": "normalized-xyxy-risk-boxes-v1",
}
),
("source.camera.rgb",),
("detection.boxes",),
),
ModuleSpec(
"simulation-waypoint-mission",
"Маршрут · контроль продвижения",
"policy",
nav["image"].removeprefix("sha256:"),
digest(
{
"policy": hashlib.sha256(
Path(__file__).with_name("mission_policy.py").read_bytes()
).hexdigest(),
"adapter": hashlib.sha256(
(adapter_root / "navigation_client.py").read_bytes()
).hexdigest(),
}
),
None,
digest(
{
"task": "operator-metric-waypoints-v1",
"recovery": "three-observed-reverse-and-replan-attempts-v2",
}
),
(
"segmentation.surface",
"source.camera.calibration",
"source.lidar",
"source.pose",
"source.simulation-time",
),
("navigation.goal", "navigation.intent"),
state_policy="causal-reset-at-source-start",
),
ModuleSpec(
"simulation-cmu-navigation",
"CMU · рельеф и движение",
"motion",
nav["image"].removeprefix("sha256:"),
digest(
{
p.relative_to(adapter_root).as_posix(): hashlib.sha256(
p.read_bytes()
).hexdigest()
for p in (
adapter_root / "navigation/server.py",
adapter_root / "navigation/footprint.py",
adapter_root / "navigation/terrain_costs.py",
adapter_root / "navigation/terrain_connectivity.cpp",
adapter_root / "navigation/fastdds.xml",
adapter_root / "navigation_client.py",
)
}
),
None,
digest(
{
"upstream": nav["upstream_commit"],
"footprint": [1, 1],
"inputs": "metric-occluded-range-and-rgb",
"command": "signed-mps-rps-observed-recovery-v2",
}
),
(
"detection.boxes",
"navigation.goal",
"navigation.intent",
"segmentation.surface",
"source.camera.calibration",
"source.lidar",
"source.pose",
),
("motion.command", "motion.path"),
state_policy="causal-reset-at-source-start",
),
)
)
def compose(adapter_root: Path, selection=None):
installed = registry(adapter_root)
if selection is None:
defaults = json.loads((adapter_root / "models.worker-006.json").read_text())[
"default_modules"
]
selection = {
"schema_version": COMPOSITION_SCHEMA,
"selections": [
{
"group": m.group,
"module_id": m.module_id,
"module_sha256": m.sha256,
"parameters": {},
}
for m in installed.modules
if m.module_id in defaults
],
}
result = installed.compose(selection, execution_mode="worker-local-simulation")
if "motion.command" not in result.outputs:
raise CompositionError("Для движения выберите модуль навигации и его зависимости.")
return result
@@ -0,0 +1,194 @@
"""Versioned scene preparation and camera-driven run contracts."""
from ipaddress import ip_address, ip_network
from typing import Annotated, Literal
from pydantic import BaseModel, ConfigDict, Field, HttpUrl, field_validator
class Contract(BaseModel):
model_config = ConfigDict(extra="forbid", allow_inf_nan=False, str_strip_whitespace=True)
class WorldCreate(Contract):
name: str = Field(min_length=1, max_length=120)
filename: str = Field(pattern=r"^[^/\\\x00-\x1f]{1,200}\.[pP][lL][yY]$")
byte_length: int = Field(gt=0, le=8 * 1024**3, strict=True)
author: str = Field(min_length=1, max_length=160)
license: str = Field(min_length=1, max_length=160)
source_url: HttpUrl | None = None
class WorldSettings(Contract):
# Source -> metric Z-up world; collision proxy is prepared separately.
meters_per_unit: float = Field(default=1, ge=0.0001, le=1000)
rotation_degrees: tuple[
Annotated[float, Field(ge=-360, le=360)],
Annotated[float, Field(ge=-360, le=360)],
Annotated[float, Field(ge=-360, le=360)],
] = (0, 0, 0)
ground_z: float = Field(default=0, ge=-10000, le=10000)
spawn_xy: tuple[
Annotated[float, Field(ge=-10000, le=10000)], Annotated[float, Field(ge=-10000, le=10000)]
] = (0, 0)
heading_degrees: float = Field(default=0, ge=-360, le=360)
camera_height_m: float = Field(default=0.5, ge=0.1, le=3)
max_speed_mps: float = Field(default=0.3, ge=0.05, le=1)
prepared: bool = False
route_xy: list[
tuple[
Annotated[float, Field(ge=-10000, le=10000)],
Annotated[float, Field(ge=-10000, le=10000)],
]
] = Field(default_factory=list, max_length=32)
class RunCreate(Contract):
world_id: str = Field(pattern=r"^aiworld-[a-f0-9]{32}$")
max_steps: int = Field(default=600, ge=1, le=3600, strict=True)
clock: Literal["lockstep", "realtime"] = "lockstep"
start_paused: bool = False
duration_seconds: int = Field(default=1800, ge=10, le=7200, strict=True)
composition: dict | None = None
class WorkerWorldCreate(WorldCreate):
"""Attestation from the connected Worker after validating its local files."""
sha256: str = Field(pattern=r"^[a-f0-9]{64}$")
collider_sha256: str = Field(pattern=r"^[a-f0-9]{64}$")
splat_count: int = Field(ge=1, le=20_000_000, strict=True)
settings: WorldSettings
class StreamEndpoint(Contract):
server: str
signaling_port: Literal[49100] = 49100
media_port: Literal[47998] = 47998
width: Literal[1280] = 1280
height: Literal[720] = 720
fps: Literal[30] = 30
@field_validator("server")
@classmethod
def private_address(cls, value: str) -> str:
address = ip_address(value)
if (
address.version != 4
or not (address.is_private or address in ip_network("100.64.0.0/10"))
or address.is_unspecified
or address.is_multicast
):
raise ValueError("Streaming requires a private IPv4 address")
return str(address)
class WorkerHello(Contract):
worker_id: str = Field(pattern=r"^[a-zA-Z0-9_-]{1,80}$")
instance_id: str = Field(pattern=r"^[a-f0-9]{32}$")
runtime: Literal["isaac-sim-6.1"]
model_ids: list[Annotated[str, Field(min_length=1, max_length=100)]] = Field(
min_length=1, max_length=8
)
runtime_sources: dict[
Literal["worker", "scene", "models", "robot"],
Annotated[str, Field(pattern=r"^[a-f0-9]{64}$")],
] = Field(min_length=4, max_length=4)
profile_sha256: str = Field(pattern=r"^[a-f0-9]{64}$")
execution_modes: list[Literal["lockstep", "realtime"]] = ["lockstep"]
stream: StreamEndpoint | None = None
class Decision(Contract):
speed_mps: float = Field(ge=-1, le=1)
yaw_rate_rps: float = Field(ge=-1, le=1)
reason: Literal[
"road",
"obstacle",
"no-road",
"uncertain",
"inference-error",
"replanning",
"stuck",
"goal-reached",
"unstable",
"waiting",
]
road_fraction: float = Field(ge=0, le=1)
obstacle_count: int = Field(ge=0, le=300, strict=True)
class RunSample(Contract):
sequence: int = Field(ge=0, le=3600, strict=True)
simulation_time_ns: int = Field(ge=0, strict=True)
inference_ms: float = Field(ge=0, le=120000)
# The snapshot is an observation BEFORE the action below is applied.
pose_xy: tuple[float, float]
decision: Decision
image_jpeg_base64: str = Field(max_length=2 * 1024 * 1024)
class WorkerPoll(Contract):
instance_id: str = Field(pattern=r"^[a-f0-9]{32}$")
run_id: str | None = Field(default=None, pattern=r"^airun-[a-f0-9]{32}$")
class RunProgress(Contract):
phase: Literal["world", "models", "scene"]
class RunApplied(Contract):
sequence: int = Field(ge=0, le=3600, strict=True)
simulation_time_ns: int = Field(ge=0, strict=True)
physics_steps: Literal[6]
pose_xy: tuple[float, float]
pose_yaw: float | None = None
cycle_ms: float | None = Field(default=None, ge=0, le=120000)
render_ms: float | None = Field(default=None, ge=0, le=120000)
transport_ms: float | None = Field(default=None, ge=0, le=120000)
class WorkerResult(Contract):
instance_id: str = Field(pattern=r"^[a-f0-9]{32}$")
outcome: Literal["completed", "stopped", "failed"]
message: str = Field(default="", max_length=500)
resources_released: Literal[True]
class ViewControl(Contract):
camera: Literal["follow", "overview", "camera"]
class RealtimeSnapshot(Contract):
"""Bounded observation of Worker-owned state; never a physics-step receipt."""
sequence: int = Field(ge=0, strict=True)
control_sequence: int = Field(ge=0, strict=True)
state: Literal["ready", "running", "paused", "stopping"]
phase: Literal["scene", "models", "running"]
simulation_time_ns: int = Field(ge=0, strict=True)
wall_elapsed_seconds: float = Field(ge=0)
physics_steps: int = Field(ge=0, strict=True)
render_frames: int = Field(ge=0, strict=True)
sensor_frames: int = Field(ge=0, strict=True)
inference_count: int = Field(ge=0, strict=True)
dropped_frames: int = Field(ge=0, strict=True)
rtf: float = Field(ge=0, le=100)
render_fps: float = Field(ge=0, le=1000)
sensor_fps: float = Field(ge=0, le=1000)
ai_hz: float = Field(ge=0, le=1000)
inference_ms: float | None = Field(default=None, ge=0)
frame_age_ms: float | None = Field(default=None, ge=0)
command_age_ms: float | None = Field(default=None, ge=0)
pose_xy: tuple[float, float]
pose_yaw: float
speed_mps: float
applied_speed_mps: float = Field(ge=-1, le=1)
applied_yaw_rate_rps: float = Field(ge=-1, le=1)
stop_reason: Literal[
"none", "paused", "stale-camera", "stale-command", "inference-error", "unstable"
]
decision: Decision | None = None
ai_ready: bool
stream_ready: bool
camera: Literal["follow", "overview", "camera"]
@@ -0,0 +1,158 @@
"""Adapters for the pinned reference DDRNet and RF-DETR model contracts.
The simulation RGB pinhole profile is separate from the device's KB4 profile.
No device valid-FOV mask or recorded LiDAR calibration is applied to simulator RGB.
"""
import csv
import http.client
from pathlib import Path
from urllib.parse import urlsplit
import numpy as np
from PIL import Image
from k1link.perception.rf_detr_object_detector import (
COCO_SPARSE_TO_CONTIGUOUS,
RISK_CLASS_IDS,
TritonRfDetrHttpInferenceBackend,
preprocess_raw_kb4_rf_detr,
)
MODEL_IDS = ["ddrnet-goose-pytorch-reference", "rf_detr_large"]
GOOSE_SURFACE_IDS = (3, 5, 7, 9, 11, 18, 21, 23, 24, 31, 40, 50, 62)
def local_endpoint(endpoint: str):
parsed = urlsplit(endpoint)
if (
parsed.scheme != "http"
or parsed.hostname not in {"127.0.0.1", "localhost", "::1"}
or parsed.path not in {"", "/"}
or parsed.query
or parsed.fragment
or parsed.username
):
raise ValueError("Use a local Triton endpoint or a loopback tunnel")
return parsed
class PillowResizer:
def resize(self, image, width, height):
return np.asarray(Image.fromarray(image).resize((width, height), Image.Resampling.BILINEAR))
class ModelInference:
def __init__(
self,
endpoint: str,
goose_labels: Path,
ddrnet_endpoint: str,
*,
segmenter_id: str = "simulation-ddrnet-goose",
):
if segmenter_id not in {"simulation-ddrnet-goose", "simulation-segformer-ade"}:
raise ValueError("Uninstalled surface provider")
self.segmenter_id = segmenter_id
parsed = local_endpoint(ddrnet_endpoint)
local_endpoint(endpoint)
with goose_labels.open(newline="") as stream:
labels = {int(row["label_key"]): row["class_name"] for row in csv.DictReader(stream)}
if set(labels) != set(range(64)):
raise ValueError("Expected the existing GOOSE fine-64 label table")
self.road_ids = [
i
for i, name in labels.items()
if name in {"asphalt", "bikeway", "cobble", "sidewalk", "gravel", "soil"}
]
if len(self.road_ids) != 6:
raise ValueError("GOOSE road label mapping is incomplete")
self.connection = http.client.HTTPConnection(
parsed.hostname, parsed.port or 8000, timeout=10
)
self.detector = TritonRfDetrHttpInferenceBackend(endpoint, timeout_seconds=10)
self.detector_url = local_endpoint(endpoint)
def ready(self):
self.connection.request("GET", "/ready")
response = self.connection.getresponse()
response.read(65536)
if response.status != 200:
raise RuntimeError("Selected surface provider is unavailable")
connection = http.client.HTTPConnection(
self.detector_url.hostname, self.detector_url.port or 8000, timeout=10
)
try:
connection.request("GET", "/v2/models/rf_detr_large/versions/1/ready")
response = connection.getresponse()
response.read(65536)
if response.status != 200:
raise RuntimeError("RF-DETR is unavailable")
finally:
connection.close()
def segment(self, rgb: np.ndarray):
return self.surface(rgb)["segmentation.labels"]
def surface(self, rgb: np.ndarray):
if rgb.shape != (600, 800, 3) or rgb.dtype != np.uint8:
raise ValueError("Expected an 800x600 RGB simulation camera")
self.connection.request(
"POST",
"/infer",
body=np.ascontiguousarray(rgb).tobytes(),
headers={"Content-Type": "application/octet-stream"},
)
response = self.connection.getresponse()
ade = self.segmenter_id == "simulation-segformer-ade"
expected = 512**2 * (2 if ade else 1)
raw = response.read(expected + 1)
if response.status != 200 or len(raw) != expected:
raise RuntimeError("Surface provider response changed")
mask = np.frombuffer(raw[: 512**2], dtype=np.uint8).reshape(512, 512)
if np.any(mask >= (150 if ade else 64)):
raise RuntimeError("Surface label taxonomy changed")
if ade:
candidate = np.frombuffer(raw[512**2 :], dtype=np.uint8).reshape(512, 512)
if np.any(candidate > 1):
raise RuntimeError("Surface candidate raster changed")
candidate = candidate.astype(bool)
else:
candidate = np.isin(mask, GOOSE_SURFACE_IDS)
return {"segmentation.labels": mask.copy(), "segmentation.surface": candidate}
def detect(self, rgb: np.ndarray):
if rgb.shape != (600, 800, 3) or rgb.dtype != np.uint8:
raise ValueError("Expected an 800x600 RGB simulation camera")
bgr = np.ascontiguousarray(rgb[:, :, ::-1])
detector_tensor = preprocess_raw_kb4_rf_detr(
bgr, np.ones((600, 800), dtype=bool), resizer=PillowResizer()
)
output = self.detector.infer(detector_tensor)
if (
output.boxes.shape != (1, 300, 4)
or output.logits.shape != (1, 300, 91)
or not np.isfinite(output.boxes).all()
or not np.isfinite(output.logits).all()
):
raise RuntimeError("RF-DETR output contract changed")
probabilities = 1 / (1 + np.exp(-np.clip(output.logits[0].astype(np.float32), -80, 80)))
classes = [
key for key, value in COCO_SPARSE_TO_CONTIGUOUS.items() if value in RISK_CLASS_IDS
]
scores = probabilities[:, classes].max(axis=1)
boxes = []
for x, y, width, height in output.boxes[0][scores >= 0.25]:
if width <= 0 or height <= 0:
continue
# Unlike the recorded diagnostic filter, never discard a very large near obstacle.
box = np.clip([x - width / 2, y - height / 2, x + width / 2, y + height / 2], 0, 1)
boxes.append(tuple(float(value) for value in box))
return boxes
def infer(self, rgb: np.ndarray):
return np.isin(self.segment(rgb), self.road_ids), self.detect(rgb)
def close(self):
self.connection.close()
self.detector.close()
@@ -0,0 +1,116 @@
"""Bounded waypoint mission and progress recovery, driven only by observations.
Operator waypoints specify the task. They are not a collision map or a motion
trajectory. CMU retains authority to reject every requested local waypoint.
"""
import math
import numpy as np
def inclination(pose):
x, y = pose[3:5]
return math.degrees(math.acos(max(-1, min(1, 1 - 2 * (x * x + y * y)))))
class WaypointMission:
def __init__(self, route):
self.route = [list(p) for p in route]
self.index = self.attempts = 0
self.anchor = self.anchor_time = None
self.last_time = None
self.state = "following"
self.failed_goals = []
self.goal = None
self.fault = None
self.best_distance = None
self.recovery_goal = None
self.recovery_started = None
def resume(self):
# Pause freezes the world clock. Preserve the mission cursor and any
# latched failure. The paused world has not moved: retain the observed
# goal entering the camera blind strip, but revalidate it before motion.
self.anchor = self.anchor_time = None
def update(self, pose, seconds, choose_goal, choose_recovery=None):
xy = np.asarray(pose[:2])
if self.last_time is not None and seconds < self.last_time:
raise ValueError("Mission observations must not rewind")
self.last_time = seconds
if inclination(pose) >= 30:
self.fault = "unstable"
if self.fault:
return None, self.intent(self.fault)
if self.route and np.linalg.norm(xy - self.route[self.index]) <= 0.4:
if self.index == len(self.route) - 1:
self.fault = "goal-reached"
return None, self.intent(self.fault)
self.index += 1
self.anchor = self.goal = None
self.best_distance = None
self.recovery_goal = None
self.attempts = 0
target = self.route[self.index] if self.route else None
if self.anchor is None:
self.anchor, self.anchor_time = xy.copy(), seconds
distance = float(np.linalg.norm(xy - target)) if target is not None else None
if self.best_distance is None:
self.best_distance = distance
progress = self.best_distance - distance if target is not None else 0.0
if target is None and self.goal is not None:
direction = np.asarray(self.goal[:2]) - self.anchor
progress = float(
np.dot(xy - self.anchor, direction) / max(np.linalg.norm(direction), 0.1)
)
if self.recovery_goal is not None:
remaining = np.linalg.norm(xy - self.recovery_goal[:2])
observed = choose_recovery(self.recovery_goal) if choose_recovery is not None else None
if remaining > 0.3 and seconds - self.recovery_started < 6 and observed is not None:
return self.recovery_goal, self.intent("reversing")
# Recovery is an attempt to escape, never route progress. The next
# forward attempt must beat the previous best distance to the task.
self.recovery_goal = self.goal = None
self.anchor_time = seconds
return None, self.intent("replanning")
if progress >= 0.1:
# A reverse/forward cycle cannot replenish the recovery budget.
self.anchor, self.anchor_time = xy.copy(), seconds
self.best_distance = distance
self.attempts = 0
self.failed_goals.clear()
stalled = seconds - self.anchor_time >= 8
if stalled:
self.attempts += 1
if self.goal is not None:
self.failed_goals.append(self.goal)
self.goal = None
self.anchor_time = seconds
if self.attempts > 3:
self.fault = "stuck"
return None, self.intent("stuck")
self.state = "replanning"
if choose_recovery is not None:
self.recovery_goal = choose_recovery(None)
if self.recovery_goal is not None:
self.recovery_started = seconds
return self.recovery_goal, self.intent("reversing")
observed = choose_goal(target, self.goal, self.failed_goals)
if observed is None:
# A rejected frame stops motion, not causal memory. Forgetting the
# last observed exact waypoint strands it behind the near camera
# boundary on the next frame. choose_goal must revalidate visible
# support, and live range/collision checks retain final authority.
return None, self.intent("no-road")
self.goal = observed
self.state = "replanning" if self.attempts else "following"
return self.goal, self.intent(self.state)
def intent(self, state):
return dict(
state=state,
waypoint=self.index,
waypoint_count=len(self.route),
recovery_attempt=self.attempts,
)
@@ -0,0 +1,79 @@
"""Camera-only laboratory road follower. No scene truth or actor coordinates enter here.
This deliberately small baseline measures model-driven following/braking. It is
not a route planner, metric obstacle-distance estimator, or field safety policy.
"""
import numpy as np
from k1link.simulation.ai_polygon.contracts import Decision
class RoadPolicy:
def __init__(self, max_speed_mps: float):
if not 0 < max_speed_mps <= 1:
raise ValueError("invalid laboratory speed")
self.max_speed = max_speed_mps
self.clear_frames = 0
def reset(self):
self.clear_frames = 0
def decide(
self, road_mask: np.ndarray, boxes: list[tuple[float, float, float, float]]
) -> Decision:
if road_mask.shape != (512, 512) or road_mask.dtype != np.bool_:
raise ValueError("road mask must be the model's 512x512 boolean raster")
# Detector boxes are normalized in the full RGB camera, not the DDRNet crop.
if any(
not all(np.isfinite(box))
or not (0 <= box[0] <= box[2] <= 1 and 0 <= box[1] <= box[3] <= 1)
for box in boxes
):
raise ValueError("invalid observation box")
obstacles = sum(x1 < 0.68 and x2 > 0.32 and y2 > 0.55 for x1, _, x2, y2 in boxes)
near_road = road_mask[320:500, 100:412]
fraction = float(near_road.mean())
stop = "obstacle" if obstacles else "no-road" if fraction < 0.35 else None
if stop:
self.clear_frames = 0
return Decision(
speed_mps=0,
yaw_rate_rps=0,
reason=stop,
road_fraction=fraction,
obstacle_count=obstacles,
)
self.clear_frames += 1
if self.clear_frames < 3:
return Decision(
speed_mps=0,
yaw_rate_rps=0,
reason="uncertain",
road_fraction=fraction,
obstacle_count=0,
)
# Choose a contiguous visible corridor, rather than averaging two disconnected roads.
columns = near_road.mean(axis=0) >= 0.6
edges = np.flatnonzero(np.diff(np.r_[False, columns, False].astype(np.int8)))
spans = [(a, b) for a, b in zip(edges[::2], edges[1::2], strict=True) if b - a >= 48]
if not spans:
self.clear_frames = 0
return Decision(
speed_mps=0,
yaw_rate_rps=0,
reason="uncertain",
road_fraction=fraction,
obstacle_count=0,
)
left, right = min(spans, key=lambda span: abs((span[0] + span[1]) / 2 - 156))
center = (left + right) / 2
yaw = float(np.clip((156 - center) / 156, -0.6, 0.6))
speed = self.max_speed * min(1, fraction / 0.65) * (1 - abs(yaw))
return Decision(
speed_mps=float(speed),
yaw_rate_rps=yaw,
reason="road",
road_fraction=fraction,
obstacle_count=0,
)
+412
View File
@@ -0,0 +1,412 @@
"""Single-worker laboratory lifecycle and durable observation/decision journal."""
import base64
import hashlib
import io
import json
import os
import re
import secrets
import shutil
import stat
import threading
import time
from pathlib import Path
from uuid import uuid4
from PIL import Image
from k1link.artifacts import utc_now_iso
from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue
from k1link.simulation.ai_polygon.contracts import (
RealtimeSnapshot,
RunApplied,
RunCreate,
RunSample,
WorkerHello,
)
from k1link.simulation.ai_polygon.worlds import WorldStore, write_json
RUN_ID = re.compile(r"^airun-[a-f0-9]{32}$")
TERMINAL = {"completed", "stopped", "failed"}
WORKER_LEASE_SECONDS = 20
class RunStore:
def __init__(self, worlds: WorldStore, queue: ObservatoryRecordedJobQueue | None = None):
self.worlds = worlds
self.queue = queue
self.root = worlds.root.parent / "runs"
self.root.mkdir(mode=0o700, exist_ok=True)
self.lock = threading.RLock()
self.worker: dict | None = None
self.seen = 0.0
self.active: str | None = None
self.token_path = self.root.parent / "worker.token"
if not self.token_path.exists():
try:
fd = os.open(self.token_path, os.O_CREAT | os.O_EXCL | os.O_WRONLY, 0o600)
with os.fdopen(fd, "w") as stream:
stream.write(secrets.token_urlsafe(48))
except FileExistsError:
pass
metadata = self.token_path.lstat()
if not stat.S_ISREG(metadata.st_mode) or metadata.st_mode & 0o077:
raise ValueError("AI polygon worker token must be a private regular file")
with os.fdopen(os.open(self.token_path, os.O_RDONLY | os.O_NOFOLLOW), "r") as stream:
self.token = stream.read(513).strip()
if not 32 <= len(self.token) <= 512:
raise ValueError("AI polygon worker token is invalid")
# Keep realtime ownership uncertain until the same Worker reconciles it.
# A Core restart never proves that the remote GPU/process stopped.
for row in self.list():
if row["state"] not in TERMINAL:
if row.get("clock") == "realtime":
if self.active is not None:
raise RuntimeError("Multiple unreconciled simulation owners")
self.active = row["run_id"]
row.update(state="disconnected", message="Ожидаем состояние Worker.")
else:
row.update(state="failed", message="Связь с симуляцией прервана перезапуском.")
self._save(row)
def directory(self, run_id: str) -> Path:
if not RUN_ID.fullmatch(run_id):
raise FileNotFoundError(run_id)
path = self.root / run_id
if not path.is_dir() or path.is_symlink():
raise FileNotFoundError(run_id)
return path
def get(self, run_id: str) -> dict:
return json.loads((self.directory(run_id) / "run.json").read_text())
def list(self) -> list[dict]:
return sorted(
[
self.get(p.name)
for p in self.root.iterdir()
if RUN_ID.fullmatch(p.name) and p.is_dir() and not p.is_symlink()
],
key=lambda item: item["created_at"],
reverse=True,
)
def _save(self, doc: dict) -> None:
write_json(self.directory(doc["run_id"]) / "run.json", doc)
def _expire(self) -> None:
if self.worker is not None and time.monotonic() - self.seen > WORKER_LEASE_SECONDS:
if self.active:
row = self.get(self.active)
if row.get("clock") == "realtime":
row.update(
state="disconnected",
message="Связь с Worker потеряна. Состояние уточняется.",
)
else:
row.update(state="failed", message="Worker потерял связь. Прогон остановлен.")
self.active = None
self._save(row)
self.worker = None
def status(self) -> dict:
with self.lock:
self._expire()
return {
"available": self.worker is not None,
"worker": self.worker,
"active_run": self.get(self.active) if self.active else None,
}
def register(self, hello: WorkerHello) -> dict:
with self.lock:
self._expire()
if self.worker and self.worker["instance_id"] != hello.instance_id:
raise RuntimeError("Другой Worker уже подключён.")
if self.active and self.get(self.active)["worker"] != hello.model_dump():
raise RuntimeError("Нельзя менять модели во время прогона.")
self.worker = hello.model_dump()
self.seen = time.monotonic()
return {"registered": True}
def heartbeat(self, instance_id: str) -> dict:
with self.lock:
self._require_worker(instance_id)
self.seen = time.monotonic()
return {"alive": True}
def _require_worker(self, instance_id: str) -> None:
self._expire()
if self.worker is None or self.worker["instance_id"] != instance_id:
raise RuntimeError("Сессия Worker истекла; требуется переподключение.")
def start(self, request: RunCreate, request_id: str) -> dict:
if not re.fullmatch(r"[a-zA-Z0-9_-]{8,100}", request_id):
raise ValueError("Требуется уникальный идентификатор запуска.")
with self.lock:
self._expire()
for old in self.list():
if old["request_id"] == request_id:
if old["request"] != request.model_dump():
raise RuntimeError("Идентификатор запуска уже использован.")
return old
if self.worker is None:
raise RuntimeError("Подключите Worker с симуляцией и моделями inference.")
if request.clock not in self.worker.get("execution_modes", ["lockstep"]):
raise RuntimeError("Worker не поддерживает этот режим симуляции.")
if request.clock == "realtime" and not self.worker.get("stream"):
raise RuntimeError("Видеопоток Worker не настроен.")
if self.queue is None:
raise RuntimeError("Контроль занятости Worker недоступен.")
if self.active is not None:
raise RuntimeError("Сначала завершите текущий прогон.")
world = self.worlds.get(request.world_id)
storage = world.get("storage", {})
if storage.get("kind") == "worker" and (
storage["worker_id"] != self.worker["worker_id"] or request.clock != "realtime"
):
raise RuntimeError("Локация доступна только на подготовленном Worker.")
if world["status"] != "available" or not world["settings"]["prepared"]:
raise RuntimeError("Сначала проверьте масштаб, грунт и старт ровера.")
run_id = f"airun-{uuid4().hex}"
if (
request.clock == "lockstep"
and shutil.disk_usage(self.root).free < request.max_steps * 1024**2 + 512 * 1024**2
):
raise ValueError("Недостаточно места для кадров прогона.")
(self.root / run_id).mkdir(mode=0o700)
if request.clock == "lockstep":
(self.root / run_id / "frames").mkdir(mode=0o700)
row = {
"schema_version": "missioncore.ai-polygon-run/v1",
"run_id": run_id,
"request_id": request_id,
"request": request.model_dump(),
"created_at": utc_now_iso(),
"world": world,
"worker": dict(self.worker),
"state": "starting",
"control": "pause" if request.start_paused else "play",
"control_sequence": 0,
"camera": "follow",
"telemetry": None,
"step_budget": 0,
"samples": 0,
"applied_steps": 0,
"last_applied": None,
"last_sample": None,
"message": None,
"phase": "world",
"authority": "virtual-only",
"clock": request.clock,
"step_ns": 100_000_000,
}
self._save(row)
try:
self.queue.reserve_simulation(run_id)
except RuntimeError:
row.update(state="failed", message="Worker занят другой задачей.")
self._save(row)
raise
self.active = run_id
return row
def control(self, run_id: str, command: str) -> dict:
with self.lock:
self._expire()
row = self.get(run_id)
if row["state"] in TERMINAL:
return row
if command not in {"pause", "play", "step", "stop"}:
raise ValueError("Неизвестная команда симуляции.")
if command == "step" and row.get("clock") == "realtime":
raise RuntimeError("Realtime не допускает пошаговое продвижение времени.")
if command == "step" and row["state"] != "paused":
raise RuntimeError("Один шаг доступен только на паузе.")
if row["control"] == "stop":
return row
if row["control"] == command:
return row
row["control_sequence"] = row.get("control_sequence", 0) + 1
row["control"] = "pause" if command == "step" else command
row["step_budget"] = 1 if command == "step" else 0
# Paused is acknowledged by the Worker, not inferred from this request.
if command == "stop":
row["state"] = "stopping"
self._save(row)
return row
def poll(self, instance_id: str, run_id: str | None) -> dict:
with self.lock:
self._require_worker(instance_id)
self.seen = time.monotonic()
if self.active is None:
return {"run": None, "action": "idle"}
row = self.get(self.active)
if run_id is None:
return {"run": row, "action": "load"}
if run_id != self.active:
raise RuntimeError("Прогон Worker не совпадает с активным.")
action = row["control"]
if row.get("clock") == "realtime":
# Only actual local snapshots can acknowledge pause/play.
return {"run": row, "action": action}
if action == "pause":
if row["step_budget"]:
row["step_budget"] = 0
action = "step"
else:
row["state"] = "paused"
elif action == "play" and row["samples"]:
row["state"] = "running"
self._save(row)
return {"run": row, "action": action}
def progress(self, run_id: str, instance_id: str, phase: str) -> dict:
with self.lock:
self._require_worker(instance_id)
if run_id != self.active:
raise RuntimeError("Прогон уже завершён.")
row = self.get(run_id)
if row["state"] == "starting":
row["phase"] = phase
self._save(row)
return {"control": row["control"]}
def view(self, run_id: str, camera: str) -> dict:
with self.lock:
row = self.get(run_id)
if run_id != self.active or row.get("clock") != "realtime":
raise RuntimeError("Симуляция не запущена.")
if camera not in {"follow", "overview", "camera"}:
raise ValueError("Неизвестная камера.")
row["camera"] = camera
self._save(row)
return row
def snapshot(self, run_id: str, instance_id: str, snapshot: RealtimeSnapshot) -> dict:
with self.lock:
self._require_worker(instance_id)
row = self.get(run_id)
if self.active != run_id or row.get("clock") != "realtime":
raise RuntimeError("Прогон не принадлежит realtime Worker.")
if row["worker"]["instance_id"] != instance_id:
raise RuntimeError("Прогон принадлежит другому Worker.")
old = row.get("telemetry")
if old and snapshot.sequence <= old["sequence"]:
return {
"recorded": old["sequence"]
} # Same snapshot may be retried after reconnect.
if snapshot.control_sequence > row.get("control_sequence", 0):
raise ValueError("Worker подтвердил неизвестную команду.")
if old and snapshot.simulation_time_ns < old["simulation_time_ns"]:
raise ValueError("Время Worker не может идти назад.")
if abs(snapshot.applied_speed_mps) > row["world"]["settings"]["max_speed_mps"]:
raise ValueError("Команда превышает скорость прогона.")
row.update(
telemetry=snapshot.model_dump(),
phase=snapshot.phase,
samples=snapshot.inference_count,
message=None,
)
if row["control"] != "stop":
row["state"] = snapshot.state
row["telemetry_received_at"] = utc_now_iso()
self._save(row)
self.seen = time.monotonic()
return {"recorded": snapshot.sequence}
def sample(self, run_id: str, instance_id: str, sample: RunSample) -> dict:
with self.lock:
self._require_worker(instance_id)
if run_id != self.active:
raise RuntimeError("Прогон уже завершён.")
row = self.get(run_id)
if row.get("clock") != "lockstep":
raise RuntimeError("Realtime кадры хранятся только на Worker.")
if row["control"] == "stop":
raise RuntimeError("Получена команда остановки.")
if sample.sequence != row["samples"] or sample.sequence >= row["request"]["max_steps"]:
raise RuntimeError("Нарушена последовательность кадров.")
if row["samples"] != row["applied_steps"]:
raise RuntimeError("Предыдущий шаг физики не подтверждён.")
if sample.simulation_time_ns != sample.sequence * row["step_ns"]:
raise ValueError("Время кадра не соответствует шагу симуляции.")
if abs(sample.decision.speed_mps) > row["world"]["settings"]["max_speed_mps"]:
raise ValueError("Команда превышает скорость прогона.")
try:
image = base64.b64decode(sample.image_jpeg_base64, validate=True)
if not 4 <= len(image) <= 1024**2:
raise ValueError()
with Image.open(io.BytesIO(image)) as decoded:
if decoded.format != "JPEG" or decoded.size != (800, 600):
raise ValueError()
decoded.verify()
except Exception as exc:
raise ValueError("Неверный кадр камеры.") from exc
directory = self.directory(run_id)
if shutil.disk_usage(directory).free < len(image) + 512 * 1024**2:
raise ValueError("Недостаточно места для кадра прогона.")
filename = f"{sample.sequence:06d}.jpg"
(directory / "frames" / filename).write_bytes(image)
recorded = sample.model_dump(exclude={"image_jpeg_base64"})
recorded.update(image_sha256=hashlib.sha256(image).hexdigest(), image=filename)
with (directory / "decisions.jsonl").open("a", encoding="utf-8") as stream:
stream.write(json.dumps(recorded, allow_nan=False) + "\n")
stream.flush()
os.fsync(stream.fileno())
row.update(samples=row["samples"] + 1, last_sample=recorded, phase="running")
if row["state"] == "starting":
row["state"] = "running"
self._save(row)
return {"recorded": sample.sequence}
def applied(self, run_id: str, instance_id: str, receipt: RunApplied) -> dict:
with self.lock:
self._require_worker(instance_id)
if self.active != run_id:
raise RuntimeError("Прогон уже завершён.")
row = self.get(run_id)
if row.get("clock") != "lockstep":
raise RuntimeError("Realtime не использует подтверждения физических шагов.")
if receipt.sequence != row["applied_steps"] or row["samples"] != receipt.sequence + 1:
raise RuntimeError("Шаг не соответствует решению модели.")
if receipt.simulation_time_ns != (receipt.sequence + 1) * row["step_ns"]:
raise ValueError("Неверное время завершения шага.")
recorded = receipt.model_dump()
with (self.directory(run_id) / "motion.jsonl").open("a", encoding="utf-8") as stream:
stream.write(json.dumps(recorded, allow_nan=False) + "\n")
stream.flush()
os.fsync(stream.fileno())
row.update(applied_steps=row["applied_steps"] + 1, last_applied=recorded)
self._save(row)
return {"applied": receipt.sequence}
def finish(self, run_id: str, instance_id: str, outcome: str, message: str) -> dict:
with self.lock:
self._require_worker(instance_id)
row = self.get(run_id)
if row["worker"]["instance_id"] != instance_id:
raise RuntimeError("Прогон принадлежит другому Worker.")
if row["state"] in TERMINAL:
if self.queue is not None:
self.queue.release_simulation(run_id)
return row
if run_id != self.active:
raise RuntimeError("Прогон уже завершён.")
if (
row.get("clock") == "lockstep"
and outcome == "completed"
and (
row["samples"] != row["request"]["max_steps"]
or row["applied_steps"] != row["samples"]
)
):
raise RuntimeError("Прогон не достиг заданного числа шагов.")
row.update(state=outcome, message=message or None)
self._save(row)
if self.queue is not None:
self.queue.release_simulation(run_id)
self.active = None
return row
@@ -0,0 +1,24 @@
"""Collision identity is independent of episode start, heading and camera."""
def terrain_matches(manifest, world, generator_sha256=None):
if manifest.get("source_sha256") != world["sha256"]:
return False
if generator_sha256 and manifest.get("generator_sha256") != generator_sha256:
return False
old, new = manifest["settings"], world["settings"]
if any(old[key] != new[key] for key in ("meters_per_unit", "rotation_degrees")):
return False
if manifest.get("generator") == "paired-source":
# A supplied full-scene collider is not the generated 30 m tile below.
# Source/calibration and collider hashes still bind this asset; actual
# support and full-body clearance are checked at the new start by Isaac.
return manifest.get("collider_sha256") == world.get("collider_sha256") and bool(
manifest.get("collider_sha256")
)
# The versioned preparer captures a 30x30 m tile and a 12 m vertical band.
# Admit starts only within its interior; physical support is checked later.
return (
all(abs(old["spawn_xy"][i] - new["spawn_xy"][i]) <= 12 for i in (0, 1))
and abs(old["ground_z"] - new["ground_z"]) <= 2
)
+256
View File
@@ -0,0 +1,256 @@
"""Durable, resumable Gaussian asset admission, independent of XGRIDS sources."""
import hashlib
import json
import os
import re
import shutil
import threading
from pathlib import Path
from uuid import uuid4
import numpy as np
from k1link.artifacts import utc_now_iso
from k1link.simulation.ai_polygon.contracts import WorkerWorldCreate, WorldCreate, WorldSettings
CHUNK_BYTES = 4 * 1024**2
WORLD_ID = re.compile(r"^aiworld-[a-f0-9]{32}$")
SOURCES = [
{
"name": "Forest Scan",
"author": "draftmode",
"license": "CC BY 4.0",
"source_url": "https://superspl.at/scene/259c0051",
"description": "Лесная тропа и папоротники",
},
{
"name": "Bamboo Trail",
"author": "luckysplat",
"license": "CC BY 4.0",
"source_url": "https://superspl.at/scene/dd49e9a8",
"description": "Тропа в бамбуковой роще",
},
]
def write_json(path: Path, value: object) -> None:
temporary = path.with_name(f".{path.name}-{uuid4().hex}")
try:
with temporary.open("x", encoding="utf-8") as stream:
os.chmod(temporary, 0o600)
json.dump(value, stream, ensure_ascii=False, allow_nan=False)
stream.flush()
os.fsync(stream.fileno())
temporary.replace(path)
finally:
temporary.unlink(missing_ok=True)
def inspect_gaussian_ply(path: Path) -> int:
"""Admit only standard scalar binary 3DGS; do not label meshes as splats."""
with path.open("rb") as stream:
header = bytearray()
while len(header) < 65536:
line = stream.readline(1024)
header.extend(line)
if line.rstrip() == b"end_header":
break
if not line:
raise ValueError("В PLY отсутствует заголовок Gaussian-сцены.")
else:
raise ValueError("Заголовок PLY превышает допустимый размер.")
try:
lines = bytes(header).decode("ascii").splitlines()
except UnicodeDecodeError as exc:
raise ValueError("Некорректный заголовок PLY.") from exc
if lines[:2] != ["ply", "format binary_little_endian 1.0"]:
raise ValueError("Экспортируйте стандартный Gaussian PLY (binary little-endian).")
elements = [line for line in lines if line.startswith("element ")]
if len(elements) != 1 or not elements[0].startswith("element vertex "):
raise ValueError("Нужен Gaussian PLY с одним элементом vertex, без меша.")
count = int(elements[0].split()[-1])
properties = [line.split() for line in lines if line.startswith("property ")]
names = [prop[-1] for prop in properties]
required = {
"x",
"y",
"z",
"opacity",
"f_dc_0",
"f_dc_1",
"f_dc_2",
"scale_0",
"scale_1",
"scale_2",
"rot_0",
"rot_1",
"rot_2",
"rot_3",
}
if (
not 1 <= count <= 20_000_000
or not required.issubset(names)
or len(names) != len(set(names))
or not 14 <= len(properties) <= 128
or any(len(prop) != 3 or prop[1] not in {"float", "float32"} for prop in properties)
):
raise ValueError("PLY не содержит поддерживаемые Gaussian-атрибуты.")
if path.stat().st_size != len(header) + count * len(properties) * 4:
raise ValueError("Размер PLY не соответствует его заголовку.")
with path.open("rb") as stream:
stream.seek(len(header))
for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""):
if not np.isfinite(np.frombuffer(chunk, dtype="<f4")).all():
raise ValueError("PLY содержит нечисловые или бесконечные атрибуты.")
return count
class WorldStore:
def __init__(self, root: Path):
self.root = root / "ai-polygon" / "worlds"
self.root.mkdir(parents=True, exist_ok=True, mode=0o700)
self.lock = threading.RLock()
def directory(self, world_id: str) -> Path:
if not WORLD_ID.fullmatch(world_id):
raise FileNotFoundError(world_id)
path = self.root / world_id
if path.is_symlink() or not path.is_dir():
raise FileNotFoundError(world_id)
return path
def get(self, world_id: str) -> dict:
with self.lock:
path = self.directory(world_id)
document = json.loads((path / "world.json").read_text())
if document["status"] == "uploading":
source = path / "source.part"
if not source.exists():
source = path / "source.ply"
document["uploaded_bytes"] = source.stat().st_size
return document
def list(self) -> list[dict]:
return sorted(
[
self.get(p.name)
for p in self.root.iterdir()
if WORLD_ID.fullmatch(p.name) and p.is_dir() and not p.is_symlink()
],
key=lambda item: item["created_at"],
reverse=True,
)
def create(self, request: WorldCreate) -> dict:
with self.lock:
if shutil.disk_usage(self.root).free < request.byte_length + 512 * 1024**2:
raise ValueError("Недостаточно места для исходника сцены.")
world_id = f"aiworld-{uuid4().hex}"
path = self.root / world_id
path.mkdir(mode=0o700)
(path / "source.part").touch(mode=0o600)
document = {
"schema_version": "missioncore.ai-polygon-world/v1",
"world_id": world_id,
**request.model_dump(mode="json"),
"status": "uploading",
"uploaded_bytes": 0,
"sha256": None,
"splat_count": None,
"created_at": utc_now_iso(),
"settings": WorldSettings().model_dump(mode="json"),
}
write_json(path / "world.json", document)
return document
def register_worker_asset(self, request: WorkerWorldCreate, worker_id: str) -> dict:
"""Keep only an authenticated asset manifest on the operator machine."""
with self.lock:
payload = request.model_dump(mode="json")
storage = {"kind": "worker", "worker_id": worker_id}
for existing in self.list():
if existing.get("storage") == storage and existing["sha256"] == request.sha256:
if any(existing.get(k) != v for k, v in payload.items() if k != "settings"):
raise RuntimeError("Манифест сохранённой локации изменился.")
return existing
world_id = f"aiworld-{uuid4().hex}"
path = self.root / world_id
path.mkdir(mode=0o700)
document = {
"schema_version": "missioncore.ai-polygon-world/v1",
"world_id": world_id,
**payload,
"storage": storage,
"status": "available",
"uploaded_bytes": 0,
"created_at": utc_now_iso(),
}
write_json(path / "world.json", document)
return document
def append(self, world_id: str, offset: int, payload: bytes) -> dict:
with self.lock:
doc = self.get(world_id)
if doc["status"] != "uploading" or offset != doc["uploaded_bytes"]:
raise RuntimeError("Позиция загрузки изменилась. Возобновите передачу.")
if not 0 < len(payload) <= CHUNK_BYTES or offset + len(payload) > doc["byte_length"]:
raise ValueError("Размер фрагмента загрузки недопустим.")
if shutil.disk_usage(self.root).free < len(payload) + 512 * 1024**2:
raise ValueError("Недостаточно свободного места.")
part = self.directory(world_id) / "source.part"
with part.open("r+b") as stream:
stream.seek(offset)
stream.write(payload)
stream.flush()
os.fsync(stream.fileno())
return self.get(world_id)
def complete(self, world_id: str) -> dict:
with self.lock:
doc = self.get(world_id)
if doc["status"] == "available":
return doc
if doc["uploaded_bytes"] != doc["byte_length"]:
raise RuntimeError("Загрузка сцены ещё не завершена.")
directory = self.directory(world_id)
source = directory / "source.part"
# Recover an interrupted atomic publication without discarding the source.
if not source.exists():
source = directory / "source.ply"
count = inspect_gaussian_ply(source)
digest = hashlib.sha256()
with source.open("rb") as stream:
for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""):
digest.update(chunk)
source.replace(directory / "source.ply")
doc.update(status="available", sha256=digest.hexdigest(), splat_count=count)
write_json(directory / "world.json", doc)
return doc
def prefix_hashes(self, world_id: str) -> dict:
"""Verify a resumed file against every byte already admitted, in bounded chunks."""
with self.lock:
doc = self.get(world_id)
if doc.get("storage", {}).get("kind") == "worker":
raise RuntimeError("Исходник локации хранится на Worker.")
directory = self.directory(world_id)
source = directory / "source.part"
if not source.exists():
source = directory / "source.ply"
chunks = []
with source.open("rb") as stream:
for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""):
chunks.append(
{"byte_length": len(chunk), "sha256": hashlib.sha256(chunk).hexdigest()}
)
return {"uploaded_bytes": doc["uploaded_bytes"], "chunks": chunks}
def configure(self, world_id: str, settings: WorldSettings) -> dict:
with self.lock:
doc = self.get(world_id)
if doc["status"] != "available":
raise RuntimeError("Сначала завершите импорт сцены.")
doc["settings"] = settings.model_dump(mode="json")
write_json(self.directory(world_id) / "world.json", doc)
return doc
+231
View File
@@ -0,0 +1,231 @@
"""Control Station and authenticated simulation-worker ports for AI polygon."""
import secrets
from pathlib import Path
from typing import Literal
from fastapi import APIRouter, Header, HTTPException, Request, Response
from fastapi.responses import FileResponse
from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue
from k1link.simulation.ai_polygon.composition import compose, registry
from k1link.simulation.ai_polygon.contracts import (
RealtimeSnapshot,
RunApplied,
RunCreate,
RunProgress,
RunSample,
ViewControl,
WorkerHello,
WorkerPoll,
WorkerResult,
WorkerWorldCreate,
WorldCreate,
WorldSettings,
)
from k1link.simulation.ai_polygon.runs import RunStore
from k1link.simulation.ai_polygon.worlds import CHUNK_BYTES, SOURCES, WorldStore
def build_ai_polygon_router(
data_dir: Path, queue: ObservatoryRecordedJobQueue | None = None
) -> APIRouter:
worlds = WorldStore(data_dir)
runs = RunStore(worlds, queue)
router = APIRouter(prefix="/api/v1/ai-polygon", tags=["ai-polygon"])
adapters = Path(__file__).resolve().parents[3] / "simulation/ai-polygon"
def invoke(fn, *args):
try:
return fn(*args)
except FileNotFoundError as exc:
raise HTTPException(404, "Локация или прогон не найдены.") from exc
except ValueError as exc:
raise HTTPException(400, str(exc)) from exc
except RuntimeError as exc:
raise HTTPException(409, str(exc)) from exc
def authenticate(authorization: str | None) -> None:
expected = f"Bearer {runs.token}"
if not authorization or not secrets.compare_digest(authorization, expected):
raise HTTPException(401, "Worker authentication required")
@router.get("/catalog")
def catalog():
return {
"schema_version": "missioncore.ai-polygon-catalog/v1",
"sources": SOURCES,
"worlds": worlds.list(),
"runtime": runs.status(),
"runs": runs.list()[:30],
}
@router.post("/worlds", status_code=201)
def create_world(body: WorldCreate):
return invoke(worlds.create, body)
@router.get("/worlds/{world_id}")
def get_world(world_id: str):
return invoke(worlds.get, world_id)
@router.get("/worlds/{world_id}/upload-prefix")
def upload_prefix(world_id: str):
return invoke(worlds.prefix_hashes, world_id)
@router.patch("/worlds/{world_id}/source")
async def upload(
world_id: str, request: Request, upload_offset: int = Header(alias="Upload-Offset", ge=0)
):
data = bytearray()
async for block in request.stream():
if len(data) + len(block) > CHUNK_BYTES:
raise HTTPException(413, "Фрагмент загрузки превышает 4 МБ.")
data.extend(block)
return invoke(worlds.append, world_id, upload_offset, bytes(data))
@router.post("/worlds/{world_id}/complete")
def complete(world_id: str):
return invoke(worlds.complete, world_id)
@router.put("/worlds/{world_id}/settings")
def settings(world_id: str, body: WorldSettings):
return invoke(worlds.configure, world_id, body)
@router.get("/worlds/{world_id}/source.ply")
def source(world_id: str):
row = invoke(worlds.get, world_id)
if row["status"] != "available":
raise HTTPException(409, "Импорт не завершён.")
if row.get("storage", {}).get("kind") == "worker":
raise HTTPException(409, "Исходник локации хранится на Worker.")
return FileResponse(
worlds.directory(world_id) / "source.ply",
media_type="application/octet-stream",
headers={
"ETag": f'"{row["sha256"]}"',
"Cache-Control": "private, max-age=31536000, immutable",
},
)
@router.post("/runs", status_code=201)
def start(body: RunCreate, idempotency_key: str = Header(alias="Idempotency-Key")):
if body.clock == "realtime":
graph = invoke(compose, adapters, body.composition)
body = body.model_copy(update={"composition": graph.selection_document()})
return invoke(runs.start, body, idempotency_key)
@router.get("/ai-modules")
def modules():
installed = invoke(registry, adapters)
graph = invoke(compose, adapters)
return {
"catalog": {**installed.catalog(), "authority": "virtual-only"},
"selection": graph.selection_document(),
"composition_sha256": graph.sha256,
}
@router.get("/runs/{run_id}")
def get_run(run_id: str):
runs.status()
return invoke(runs.get, run_id)
@router.post("/runs/{run_id}/{command}")
def control(run_id: str, command: Literal["pause", "play", "step", "stop"]):
return invoke(runs.control, run_id, command)
@router.get("/runs/{run_id}/frames/{sequence}.jpg")
def frame(run_id: str, sequence: int):
row = invoke(runs.get, run_id)
if not 0 <= sequence < row["samples"]:
raise HTTPException(404, "Кадр не найден.")
return FileResponse(
runs.directory(run_id) / "frames" / f"{sequence:06d}.jpg",
media_type="image/jpeg",
headers={"Cache-Control": "private, max-age=31536000, immutable"},
)
@router.put("/runs/{run_id}/view")
def view(run_id: str, body: ViewControl):
return invoke(runs.view, run_id, body.camera)
@router.get("/runs/{run_id}/decisions")
def decisions(run_id: str):
directory = invoke(runs.directory, run_id)
path = directory / "decisions.jsonl"
if not path.exists():
return Response("", media_type="application/x-ndjson")
return FileResponse(path, media_type="application/x-ndjson", filename="decisions.jsonl")
@router.post("/worker/register")
def register(body: WorkerHello, authorization: str | None = Header(default=None)):
authenticate(authorization)
return invoke(runs.register, body)
@router.post("/worker/worlds", status_code=201)
def register_worker_world(
body: WorkerWorldCreate,
authorization: str | None = Header(default=None),
instance_id: str = Header(alias="Worker-Instance"),
):
authenticate(authorization)
with runs.lock:
invoke(runs.heartbeat, instance_id)
worker = runs.status()["worker"]
return invoke(worlds.register_worker_asset, body, worker["worker_id"])
@router.post("/worker/heartbeat")
def heartbeat(body: WorkerPoll, authorization: str | None = Header(default=None)):
authenticate(authorization)
return invoke(runs.heartbeat, body.instance_id)
@router.post("/worker/poll")
def poll(body: WorkerPoll, authorization: str | None = Header(default=None)):
authenticate(authorization)
return invoke(runs.poll, body.instance_id, body.run_id)
@router.post("/worker/runs/{run_id}/progress")
def progress(
run_id: str,
body: RunProgress,
instance_id: str = Header(alias="Worker-Instance"),
authorization: str | None = Header(default=None),
):
authenticate(authorization)
return invoke(runs.progress, run_id, instance_id, body.phase)
@router.post("/worker/runs/{run_id}/samples")
def sample(
run_id: str,
body: RunSample,
instance_id: str = Header(alias="Worker-Instance"),
authorization: str | None = Header(default=None),
):
authenticate(authorization)
return invoke(runs.sample, run_id, instance_id, body)
@router.post("/worker/runs/{run_id}/snapshot")
def snapshot(
run_id: str,
body: RealtimeSnapshot,
instance_id: str = Header(alias="Worker-Instance"),
authorization: str | None = Header(default=None),
):
authenticate(authorization)
return invoke(runs.snapshot, run_id, instance_id, body)
@router.post("/worker/runs/{run_id}/finish")
def finish(run_id: str, body: WorkerResult, authorization: str | None = Header(default=None)):
authenticate(authorization)
return invoke(runs.finish, run_id, body.instance_id, body.outcome, body.message)
@router.post("/worker/runs/{run_id}/applied")
def applied(
run_id: str,
body: RunApplied,
instance_id: str = Header(alias="Worker-Instance"),
authorization: str | None = Header(default=None),
):
authenticate(authorization)
return invoke(runs.applied, run_id, instance_id, body)
return router
+5
View File
@@ -238,6 +238,7 @@ from k1link.missions.registration_runs import RegistrationRuns
from k1link.web.mission_registration_api import build_mission_registration_router from k1link.web.mission_registration_api import build_mission_registration_router
from k1link.web.mission_planner_api import build_mission_planner_router from k1link.web.mission_planner_api import build_mission_planner_router
from k1link.web.simulation_projects_api import build_simulation_projects_router from k1link.web.simulation_projects_api import build_simulation_projects_router
from k1link.web.ai_polygon_api import build_ai_polygon_router
from k1link.web.simulation_world_provider_api import build_simulation_world_provider_router from k1link.web.simulation_world_provider_api import build_simulation_world_provider_router
from k1link.web.system_telemetry_api import build_system_telemetry_router from k1link.web.system_telemetry_api import build_system_telemetry_router
from k1link.web.vegetation_shadow_lab_api import ( from k1link.web.vegetation_shadow_lab_api import (
@@ -1996,6 +1997,10 @@ app.include_router(
) )
) )
frontend_dist = REPOSITORY_ROOT / "apps" / "control-station" / "dist" frontend_dist = REPOSITORY_ROOT / "apps" / "control-station" / "dist"
try:
app.include_router(build_ai_polygon_router(session_store.data_dir, OBSERVATORY_RECORDED_JOB_QUEUE))
except (OSError, ValueError):
logging.getLogger(__name__).exception("AI polygon could not load its private runtime state")
app.include_router( app.include_router(
build_viewer_diagnostics_router( build_viewer_diagnostics_router(
expected_ui_build_id=lambda: frontend_build_id(frontend_dist), expected_ui_build_id=lambda: frontend_build_id(frontend_dist),
+398
View File
@@ -0,0 +1,398 @@
"""Synthetic protocol checks; these do not qualify rendering or AI model quality."""
import base64
import hashlib
import io
import json
import numpy as np
import pytest
from fastapi import FastAPI
from fastapi.testclient import TestClient
from PIL import Image
from pydantic import ValidationError
from test_observatory_recorded_jobs import _definitions
from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue
from k1link.simulation.ai_polygon.contracts import (
Decision,
RunApplied,
RunCreate,
RunSample,
WorkerHello,
WorkerWorldCreate,
WorldCreate,
WorldSettings,
)
from k1link.simulation.ai_polygon.policy import RoadPolicy
from k1link.simulation.ai_polygon.runs import RunStore
from k1link.simulation.ai_polygon.worlds import WorldStore, inspect_gaussian_ply
from k1link.web.ai_polygon_api import build_ai_polygon_router
def ply(value=0.0):
names = [
"x",
"y",
"z",
"opacity",
"f_dc_0",
"f_dc_1",
"f_dc_2",
"scale_0",
"scale_1",
"scale_2",
"rot_0",
"rot_1",
"rot_2",
"rot_3",
]
header = "ply\nformat binary_little_endian 1.0\nelement vertex 1\n"
header += "".join(f"property float {name}\n" for name in names) + "end_header\n"
return header.encode() + np.full(14, value, dtype="<f4").tobytes()
def make_world(worlds):
data = ply()
world = worlds.create(
WorldCreate(
name="Synthetic protocol fixture",
filename="fixture.PLY",
byte_length=len(data),
author="test",
license="CC0",
)
)
worlds.append(world["world_id"], 0, data)
worlds.complete(world["world_id"])
return worlds.configure(world["world_id"], WorldSettings(prepared=True))
@pytest.fixture
def runs(tmp_path):
queue = ObservatoryRecordedJobQueue(tmp_path, definitions=_definitions())
worlds = WorldStore(tmp_path)
store = RunStore(worlds, queue)
hello = WorkerHello(
worker_id="worker-006",
instance_id="a" * 32,
runtime="isaac-sim-6.1",
model_ids=["ddrnet-goose-pytorch-reference", "rf_detr_large"],
profile_sha256="b" * 64,
runtime_sources={key: "c" * 64 for key in ("worker", "scene", "models", "robot")},
)
store.register(hello)
return store, hello, make_world(worlds)
def sample(sequence=0, **changes):
stream = io.BytesIO()
Image.new("RGB", (800, 600)).save(stream, "JPEG")
row = dict(
sequence=sequence,
simulation_time_ns=sequence * 100_000_000,
inference_ms=5,
pose_xy=(0, 0),
decision=Decision(
speed_mps=0, yaw_rate_rps=0, reason="no-road", road_fraction=0, obstacle_count=0
),
image_jpeg_base64=base64.b64encode(stream.getvalue()).decode(),
)
return RunSample(**{**row, **changes})
def test_resume_and_atomic_publication_recovery(tmp_path):
worlds = WorldStore(tmp_path)
data = ply()
row = worlds.create(
WorldCreate(
name=" Test ", filename="scan.ply", byte_length=len(data), author="Test", license="CC0"
)
)
key = row["world_id"]
worlds.append(key, 0, data[:20])
assert worlds.prefix_hashes(key)["chunks"] == [
{"byte_length": 20, "sha256": hashlib.sha256(data[:20]).hexdigest()}
]
with pytest.raises(RuntimeError):
worlds.append(key, 0, data[:20])
with pytest.raises(RuntimeError):
worlds.complete(key)
worlds.append(key, 20, data[20:])
directory = worlds.directory(key)
(directory / "source.part").replace(directory / "source.ply")
recovered = WorldStore(tmp_path).complete(key)
assert recovered["status"] == "available"
assert recovered["sha256"] == hashlib.sha256(data).hexdigest()
assert recovered["name"] == "Test"
assert worlds.complete(key) == recovered
@pytest.mark.parametrize(
"data", [b"not ply", ply()[:-1], ply() + b"xxxx", ply(float("nan")), ply(float("inf"))]
)
def test_invalid_ply_rejected(tmp_path, data):
path = tmp_path / "bad.ply"
path.write_bytes(data)
with pytest.raises(ValueError):
inspect_gaussian_ply(path)
@pytest.mark.parametrize(
"settings",
[{"spawn_xy": [float("nan"), 0]}, {"rotation_degrees": [0, 0, 900]}, {"max_speed_mps": 2}],
)
def test_nonfinite_or_out_of_bounds_settings_rejected(settings):
with pytest.raises(ValidationError):
WorldSettings(**settings)
def test_start_snapshots_and_idempotency(runs):
store, hello, world = runs
request = RunCreate(world_id=world["world_id"], max_steps=2)
row = store.start(request, "request-001")
assert store.start(request, "request-001") == row
store.worlds.configure(world["world_id"], WorldSettings(max_speed_mps=0.5, prepared=True))
assert store.get(row["run_id"])["world"]["settings"]["max_speed_mps"] == 0.3
with pytest.raises(RuntimeError):
store.start(RunCreate(world_id=world["world_id"], max_steps=3), "request-001")
with pytest.raises(RuntimeError):
store.start(request, "request-002")
assert store.poll(hello.instance_id, None)["action"] == "load"
def test_sequence_clock_pause_step_stop_and_resource_release(runs):
store, hello, world = runs
key = store.start(RunCreate(world_id=world["world_id"], max_steps=2), "request-001")["run_id"]
with pytest.raises(RuntimeError):
store.control(key, "step")
store.poll(hello.instance_id, key)
store.sample(key, hello.instance_id, sample())
with pytest.raises(RuntimeError):
store.sample(key, hello.instance_id, sample())
store.applied(
key,
hello.instance_id,
RunApplied(sequence=0, simulation_time_ns=100_000_000, physics_steps=6, pose_xy=(0, 0)),
)
with pytest.raises(ValueError):
store.sample(key, hello.instance_id, sample(1, simulation_time_ns=999))
store.control(key, "pause")
assert store.get(key)["state"] == "running"
assert store.poll(hello.instance_id, key)["run"]["state"] == "paused"
store.control(key, "step")
assert store.poll(hello.instance_id, key)["action"] == "step"
assert store.poll(hello.instance_id, key)["action"] == "pause"
store.control(key, "stop")
with pytest.raises(RuntimeError):
store.sample(key, hello.instance_id, sample(1))
store.finish(key, hello.instance_id, "stopped", "")
store.queue.reserve_simulation("airun-" + "b" * 32)
def test_expired_worker_stops_run_but_does_not_claim_gpu_released(runs):
store, hello, world = runs
key = store.start(RunCreate(world_id=world["world_id"]), "request-001")["run_id"]
store.seen -= 21
assert not store.status()["available"]
assert store.get(key)["state"] == "failed"
with pytest.raises(RuntimeError):
store.sample(key, hello.instance_id, sample())
with pytest.raises(RuntimeError):
store.queue.reserve_simulation("airun-" + "b" * 32)
store.register(hello)
assert store.finish(key, hello.instance_id, "failed", "")["state"] == "failed"
store.queue.reserve_simulation("airun-" + "b" * 32)
def test_restart_fences_worker_and_preserves_journal(runs):
store, hello, world = runs
key = store.start(RunCreate(world_id=world["world_id"], max_steps=2), "request-001")["run_id"]
store.poll(hello.instance_id, key)
store.sample(key, hello.instance_id, sample())
restarted = RunStore(store.worlds, store.queue)
assert restarted.get(key)["state"] == "failed"
with pytest.raises(RuntimeError):
restarted.sample(key, hello.instance_id, sample(1))
saved = json.loads((store.directory(key) / "decisions.jsonl").read_text())
assert (
saved["image_sha256"]
== hashlib.sha256((store.directory(key) / "frames/000000.jpg").read_bytes()).hexdigest()
)
assert "image_jpeg_base64" not in saved
def test_completion_requires_expected_steps(runs):
store, hello, world = runs
key = store.start(RunCreate(world_id=world["world_id"], max_steps=1), "request-001")["run_id"]
with pytest.raises(RuntimeError):
store.finish(key, hello.instance_id, "completed", "")
store.poll(hello.instance_id, key)
store.sample(key, hello.instance_id, sample())
with pytest.raises(RuntimeError):
store.finish(key, hello.instance_id, "completed", "")
store.applied(
key,
hello.instance_id,
RunApplied(sequence=0, simulation_time_ns=100_000_000, physics_steps=6, pose_xy=(0, 0)),
)
assert store.finish(key, hello.instance_id, "completed", "")["state"] == "completed"
def test_api_auth_and_bounded_upload(tmp_path):
app = FastAPI()
app.include_router(build_ai_polygon_router(tmp_path))
with TestClient(app) as client:
root = "/api/v1/ai-polygon"
assert client.get(root + "/catalog").json()["runtime"]["available"] is False
assert client.post(root + "/worker/poll", json={"instance_id": "a" * 32}).status_code == 401
assert client.post(root + "/worlds", json={"filename": "../bad.ply"}).status_code == 422
world = make_world(WorldStore(tmp_path))
response = client.get(root + f"/worlds/{world['world_id']}/source.ply")
assert response.status_code == 200 and response.content == ply()
assert (
client.post(
root + "/runs",
headers={"Idempotency-Key": "test-run-001"},
json={"world_id": world["world_id"]},
).status_code
== 409
)
assert not any((tmp_path / "ai-polygon/runs").iterdir())
def test_camera_policy_brakes_and_waits_before_resume():
policy = RoadPolicy(0.3)
road = np.ones((512, 512), dtype=bool)
assert policy.decide(road, []).speed_mps == 0
policy.decide(road, [])
assert policy.decide(road, []).speed_mps == 0.3
obstacle = policy.decide(road, [(0.4, 0.2, 0.6, 0.8)])
assert obstacle.reason == "obstacle" and obstacle.speed_mps == 0
assert policy.decide(road, []).speed_mps == 0
assert policy.decide(np.zeros_like(road), []).reason == "no-road"
with pytest.raises(ValueError):
policy.decide(road, [(float("nan"), 0, 1, 1)])
def worker_asset():
return WorkerWorldCreate(
name="Synthetic paired scene",
filename="fixture.ply",
byte_length=len(ply()),
author="test",
license="CC0",
sha256="a" * 64,
collider_sha256="b" * 64,
splat_count=1,
settings=WorldSettings(prepared=True),
)
def test_worker_asset_is_manifest_only_and_retry_preserves_settings(tmp_path):
worlds = WorldStore(tmp_path)
request = worker_asset()
row = worlds.register_worker_asset(request, "worker-006")
assert {p.name for p in worlds.directory(row["world_id"]).iterdir()} == {"world.json"}
changed = worlds.configure(row["world_id"], WorldSettings(spawn_xy=(3, 4), prepared=True))
assert worlds.register_worker_asset(request, "worker-006") == changed
with pytest.raises(RuntimeError):
worlds.register_worker_asset(
request.model_copy(update={"collider_sha256": "c" * 64}), "worker-006"
)
with pytest.raises(RuntimeError):
worlds.prefix_hashes(row["world_id"])
def test_worker_asset_api_requires_current_authenticated_instance(tmp_path):
app = FastAPI()
app.include_router(build_ai_polygon_router(tmp_path))
base = "/api/v1/ai-polygon"
store = RunStore(WorldStore(tmp_path))
hello = WorkerHello(
worker_id="worker-006",
instance_id="a" * 32,
runtime="isaac-sim-6.1",
model_ids=["test"],
runtime_sources={k: "c" * 64 for k in ("worker", "scene", "models", "robot")},
profile_sha256="b" * 64,
)
auth = {"Authorization": "Bearer " + store.token, "Worker-Instance": hello.instance_id}
body = worker_asset().model_dump(mode="json")
with TestClient(app) as client:
assert (
client.post(
base + "/worker/worlds", json=body, headers={"Worker-Instance": hello.instance_id}
).status_code
== 401
)
assert client.post(base + "/worker/worlds", json=body, headers=auth).status_code == 409
assert (
client.post(
base + "/worker/register", json=hello.model_dump(), headers=auth
).status_code
== 200
)
response = client.post(base + "/worker/worlds", json=body, headers=auth)
assert response.status_code == 201
row = response.json()
assert row["storage"] == {"kind": "worker", "worker_id": "worker-006"}
assert client.get(base + f"/worlds/{row['world_id']}/source.ply").status_code == 409
assert (
client.post(
base + "/worker/worlds", json=body, headers={**auth, "Worker-Instance": "d" * 32}
).status_code
== 409
)
def test_camera_policy_turns_towards_connected_road():
policy = RoadPolicy(0.3)
road = np.zeros((512, 512), dtype=bool)
road[:, 100:260] = True
policy.decide(road, [])
policy.decide(road, [])
decision = policy.decide(road, [])
assert decision.yaw_rate_rps > 0 and 0 < decision.speed_mps < 0.3
def test_progress_does_not_claim_camera_ready(runs):
store, hello, world = runs
run = store.start(RunCreate(world_id=world["world_id"]), "progress-case-001")
store.progress(run["run_id"], hello.instance_id, "scene")
assert store.get(run["run_id"])["phase"] == "scene"
assert store.get(run["run_id"])["state"] == "starting"
store.control(run["run_id"], "stop")
assert store.progress(run["run_id"], hello.instance_id, "models")["control"] == "stop"
assert store.get(run["run_id"])["state"] == "stopping"
def test_worker_stop_race_accepts_already_exited_process(monkeypatch):
import importlib.util
from pathlib import Path
runtime = Path(__file__).resolve().parents[1] / "simulation/ai-polygon"
monkeypatch.syspath_prepend(str(runtime))
spec = importlib.util.spec_from_file_location("polygon_worker_stop_test", runtime / "worker.py")
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
calls = []
class Child:
pid = 123
def poll(self):
return None
def wait(self, timeout):
calls.append(("wait", timeout))
return 0
def taskkill(*args, **kwargs):
# Windows reports a non-zero taskkill when the process exits in the race.
assert kwargs["check"] is False
calls.append(("taskkill", 255))
monkeypatch.setattr(module.subprocess, "run", taskkill)
module.terminate_episode(Child())
assert calls == [("taskkill", 255), ("wait", 30)]
+141
View File
@@ -0,0 +1,141 @@
import numpy as np
import pytest
from k1link.simulation.ai_polygon.contracts import WorldSettings
from k1link.simulation.ai_polygon.mission_policy import WaypointMission, inclination
def pose(x=0, y=0):
return [x, y, 0.37, 0, 0, 0, 1]
def goal(target, prior, excluded):
return [2, 0.7 * len(excluded), 0]
def test_chassis_jitter_cannot_hide_stall_and_retries_are_bounded():
mission = WaypointMission([[8, 0]])
states = []
for t in np.arange(0, 34, 0.2):
_, intent = mission.update(pose(0.01 * np.sin(t * 10)), float(t), goal)
states.append(intent["state"])
assert "replanning" in states
assert states[-1] == "stuck"
assert mission.update(pose(1), 35, goal)[1]["state"] == "stuck"
def test_route_cursor_and_completion_survive_pause_without_restarting_task():
mission = WaypointMission([[1, 0], [3, 0]])
mission.update(pose(), 0, goal)
assert mission.update(pose(1), 6, goal)[1]["waypoint"] == 1
mission.resume()
assert mission.update(pose(1), 6, goal)[1]["waypoint"] == 1
assert mission.update(pose(3), 18, goal)[1]["state"] == "goal-reached"
mission.resume()
assert mission.update(pose(3), 18, goal)[1]["state"] == "goal-reached"
def test_overturned_and_excessive_tilt_are_latched_before_goal_success():
mission = WaypointMission([[0, 0]])
overturned = [0, 0, 0.37, 1, 0, 0, 0]
assert inclination(overturned) == pytest.approx(180)
assert mission.update(overturned, 0, goal)[1]["state"] == "unstable"
mission.resume()
assert mission.update(pose(), 1, goal)[1]["state"] == "unstable"
def test_missing_surface_never_becomes_a_drive_permission_or_unbounded_wait():
mission = WaypointMission([[8, 0]])
for t in range(34):
target, intent = mission.update(pose(), t, lambda *_: None)
assert target is None
assert intent["state"] == "stuck"
def test_route_contract_bounds_and_finiteness():
from pydantic import ValidationError
for route in ([[float("nan"), 0]], [[0, 0]] * 33, [[0, 10001]]):
with pytest.raises(ValidationError):
WorldSettings(route_xy=route)
def test_recovery_uses_observed_goal_and_does_not_count_retreat_as_progress():
mission = WaypointMission([[8, 0]])
def retreat(prior):
return prior or [-0.65, 0, 0]
mission.update(pose(), 0, goal, retreat)
target, intent = mission.update(pose(), 8, goal, retreat)
assert target == [-0.65, 0, 0] and intent["state"] == "reversing"
assert mission.update(pose(-0.2), 10, goal, retreat)[1]["state"] == "reversing"
assert mission.update(pose(-0.4), 12, goal, retreat)[1]["state"] == "replanning"
mission.update(pose(), 16, goal, retreat)
assert mission.attempts == 1
# Moving back to where we started cannot create a fresh three-attempt budget.
assert mission.update(pose(), 20, goal, retreat)[1]["recovery_attempt"] == 2
def test_recovery_stops_immediately_on_lost_support_and_times_out_without_motion():
mission = WaypointMission([[8, 0]])
def retreat(prior):
return prior or [-0.65, 0, 0]
mission.update(pose(), 0, goal, retreat)
mission.update(pose(), 8, goal, retreat)
assert mission.update(pose(), 8.2, goal, lambda _: None)[0] is None
assert mission.recovery_goal is None
for seconds in (17, 23, 32, 38, 47):
_, intent = mission.update(pose(), seconds, goal, retreat)
assert intent["state"] == "stuck"
def test_wrong_direction_never_replenishes_route_attempts():
mission = WaypointMission([[8, 0]])
for seconds in range(34):
_, intent = mission.update(pose(-seconds * 0.1), seconds, goal)
assert intent["state"] == "stuck"
def test_signed_motion_contract_accepts_reverse_but_stays_bounded():
from pydantic import ValidationError
from k1link.simulation.ai_polygon.contracts import Decision
assert (
Decision(
speed_mps=-0.1, yaw_rate_rps=0, reason="replanning", road_fraction=0, obstacle_count=0
).speed_mps
< 0
)
with pytest.raises(ValidationError):
Decision(speed_mps=-1.1, yaw_rate_rps=0, reason="road", road_fraction=1, obstacle_count=0)
def test_slow_regulated_progress_is_not_mistaken_for_stall():
mission = WaypointMission([[8, 0]])
for seconds in range(60):
_, intent = mission.update(pose(seconds * 0.02), seconds, goal)
assert intent["state"] == "following"
assert intent["recovery_attempt"] == 0
def test_rejected_observation_stops_but_does_not_forget_revalidated_goal():
mission = WaypointMission([[2, 0]])
observed = [2, 0, 0]
assert mission.update(pose(), 0, lambda *_: observed)[0] == observed
assert mission.update(pose(1), 1, lambda *_: None)[0] is None
assert mission.goal == observed
mission.resume()
calls = []
def revalidate(_target, prior, _excluded):
calls.append(prior)
return prior
assert mission.update(pose(1.3), 1.2, revalidate)[0] == observed
assert calls == [observed]
# Memory alone never authorizes a command when the new frame is rejected.
assert mission.update(pose(1.3), 1.4, lambda *_: None)[0] is None
+40
View File
@@ -0,0 +1,40 @@
"""The actuator may soften acceleration but must never prolong a safety command."""
import importlib.util
from pathlib import Path
import pytest
spec = importlib.util.spec_from_file_location(
"motion_control", Path(__file__).parents[1] / "simulation/ai-polygon/motion_control.py"
)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
def test_slow_perception_keeps_continuous_physics_command():
drive = module.DriveEnvelope()
rows = [drive.step(0.15, 0, 1 / 60)[0] for _ in range(180)]
assert rows[0] == pytest.approx(0.2 / 60)
assert all(0 <= b - a <= 0.2 / 60 + 1e-9 for a, b in zip(rows, rows[1:], strict=False))
assert rows[44:] == pytest.approx([0.15] * 136)
def test_braking_and_deadman_bypass_ramp():
drive = module.DriveEnvelope()
for _ in range(60):
drive.step(0.15, 0, 1 / 60)
assert drive.step(0.03, 0, 1 / 60) == (0.03, 0)
assert drive.step(0.15, 0.2, 1 / 60, stop=True) == (0, 0)
assert drive.wheels == (0, 0)
assert drive.step(0.15, 0, 1 / 60)[0] == pytest.approx(0.2 / 60)
def test_curvature_and_direction_change():
drive = module.DriveEnvelope()
for _ in range(60):
speed, yaw = drive.step(0.15, 0.2, 1 / 60)
assert yaw / speed == pytest.approx(0.2 / 0.15)
speed, yaw = drive.step(-0.15, -0.2, 1 / 60)
assert speed < 0 and yaw < 0
assert max(map(abs, drive.wheels)) <= 0.2 / 60 + 1e-9
+390
View File
@@ -0,0 +1,390 @@
import importlib.util
import subprocess
import sys
from pathlib import Path
import numpy as np
import pytest
from k1link.observatory.modular_composition import CompositionError
from k1link.simulation.ai_polygon.composition import compose, registry
from k1link.simulation.ai_polygon.terrain_contract import terrain_matches
ROOT = Path(__file__).resolve().parents[1] / "simulation/ai-polygon"
def module(name):
spec = importlib.util.spec_from_file_location(name, ROOT / (name + ".py"))
result = importlib.util.module_from_spec(spec)
spec.loader.exec_module(result)
return result
def test_simulation_composition_has_causal_dependencies_and_separate_authority():
graph = compose(ROOT)
assert graph.as_dict()["execution"]["mode"] == "worker-local-simulation"
motion = graph.nodes[-1]
assert motion.module.group == "motion"
assert dict(motion.inputs)["segmentation.surface"] == "simulation-segformer-ade"
assert dict(motion.inputs)["detection.boxes"] == "simulation-rf-detr"
assert motion.module.state_policy == "causal-reset-at-source-start"
selection = graph.selection_document()
selection["selections"] = [r for r in selection["selections"] if r["group"] != "segmentation"]
with pytest.raises(CompositionError, match="segmentation.surface"):
compose(ROOT, selection)
def test_surface_providers_are_interchangeable_in_the_shared_constructor():
selection = compose(ROOT).selection_document()
reference = next(m for m in registry(ROOT).modules if m.module_id == "simulation-ddrnet-goose")
for row in selection["selections"]:
if row["group"] == "segmentation":
row.update(module_id=reference.module_id, module_sha256=reference.sha256)
graph = compose(ROOT, selection)
assert dict(graph.nodes[-1].inputs)["segmentation.surface"] == reference.module_id
def test_composition_rejects_stale_module_identity():
selection = compose(ROOT).selection_document()
selection["selections"][0]["module_sha256"] = "0" * 64
with pytest.raises(CompositionError, match="not installed"):
compose(ROOT, selection)
def test_shared_constructor_import_needs_no_core_or_third_party_runtime():
# -S removes site-packages, as in the minimal Windows coordinator. Loading
# a contract must not load the POSIX-only artifact gateway through __init__.
source = str(ROOT.parents[1] / "src")
result = subprocess.run(
[
sys.executable,
"-S",
"-c",
f"import sys; sys.path.insert(0, {source!r}); "
"from k1link.observatory.modular_composition import ModuleRegistry; "
"from k1link.simulation.ai_polygon.composition import compose; "
"assert 'k1link.artifact_gateway' not in sys.modules",
],
capture_output=True,
text=True,
)
assert result.returncode == 0, result.stderr
def test_semantic_goal_uses_range_and_correct_square_camera_crop():
nav = module("navigation_client")
points = np.array(
[[x, y, 0] for x in np.linspace(1.5, 3, 20) for y in np.linspace(-0.3, 0.3, 9)],
dtype=np.float32,
)
pose = [0, 0, 0.27, 0, 0, 0, 1]
calibration = {
"origin": [0.38, 0, 0.8],
"rotation": np.eye(3).reshape(-1).tolist(),
"intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300],
}
leaves = np.ones((512, 512), dtype=bool)
goal = nav.visual_goal(leaves, points, pose, calibration)
assert goal is not None and 1.5 < goal[0] < 3 and abs(goal[1]) < 0.3
assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration) is None
assert nav.visual_goal(leaves, points + [0, 0, 2], pose, calibration) is None
# A previously valid goal cannot authorize motion through newly unknown RGB.
assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, goal) is None
# An explicit-route waypoint entering the camera blind strip is retained,
# but losing all current visual surface support still forbids movement.
close = [0.65, 0, 0]
assert nav.visual_goal(leaves, points, pose, calibration, close, target=[0.65, 0]) == close
assert (
nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, close, target=[0.65, 0])
is None
)
def test_ground_placement_uses_actual_triangle_intersection():
terrain = module("terrain")
vertices = np.array([[0, 0, 0], [1, 0, 0.2], [0, 1, 0]], dtype=np.float32)
faces = np.array([[0, 1, 2]], dtype=np.int32)
assert terrain.ground_intersections(vertices, faces, 0.25, 0.25)[0] == pytest.approx(0.05)
assert len(terrain.ground_intersections(vertices, faces, 0.9, 0.9)) == 0
def test_observed_route_goal_keeps_task_position_and_cannot_run_away_from_it():
choose = module("navigation_client").visual_goal
points = np.array(
[[x, y, 0] for x in np.arange(1.2, 3.1, 0.05) for y in np.arange(-0.5, 0.51, 0.05)]
)
pose = [0, 0, 0.37, 0, 0, 0, 1]
calibration = {
"origin": [0.38, 0, 0.8],
"rotation": np.eye(3).reshape(-1).tolist(),
"intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300],
"body_contact_height_m": 0.37,
}
surface = np.ones((512, 512), dtype=bool)
target = [2.03, 0.27]
assert choose(surface, points, pose, calibration, target=target) == pytest.approx([*target, 0])
# The close, already observed waypoint can enter the camera blind strip.
advanced = [1.5, 0, 0.37, 0, 0, 0, 1]
camera = {**calibration, "origin": [1.88, 0, 0.8]}
prior = [*target, 0]
assert choose(surface, points + [1.5, 0, 0], advanced, camera, prior, target) == prior
# The actual camera loses nearby ground beyond the old hardcoded 0.8 m.
advanced = [1.1, 0, 0.37, 0, 0, 0, 1]
camera = {**calibration, "origin": [1.48, 0, 0.8]}
assert choose(surface, points + [1.1, 0, 0], advanced, camera, prior, target) == prior
assert choose(np.zeros_like(surface), points, pose, calibration, target=target) is None
# Clear road ahead is not permission to drive away from a missed waypoint.
assert choose(surface, points, pose, calibration, target=[-1, 0]) is None
# A distant task may still use an observed local goal towards it.
far = choose(surface, points, pose, calibration, target=[8, 0])
assert far is not None and 1.2 <= far[0] <= 3.1
def test_collision_identity_follows_geometry_and_tile_coverage_not_camera_or_start():
settings = dict(
meters_per_unit=1,
rotation_degrees=[-90, 0, 180],
spawn_xy=[0, 0],
ground_z=0,
camera_height_m=0.8,
max_speed_mps=0.15,
)
terrain = dict(source_sha256="a" * 64, generator_sha256="b" * 64, settings=settings)
world = dict(sha256="a" * 64, settings={**settings, "spawn_xy": [1, 1], "camera_height_m": 1})
assert terrain_matches(terrain, world, "b" * 64)
assert not terrain_matches(terrain, world, "c" * 64)
assert not terrain_matches(terrain, {**world, "sha256": "d" * 64})
for change in ({"spawn_xy": [20, 0]}, {"meters_per_unit": 2}, {"rotation_degrees": [0, 0, 0]}):
assert not terrain_matches(terrain, {**world, "settings": {**world["settings"], **change}})
def test_paired_full_scene_does_not_inherit_generated_tile_bounds():
settings = dict(meters_per_unit=1, rotation_degrees=[90, 0, 0], spawn_xy=[0, 0], ground_z=5)
terrain = dict(
generator="paired-source",
source_sha256="a" * 64,
collider_sha256="b" * 64,
settings=settings,
)
world = dict(
sha256="a" * 64,
collider_sha256="b" * 64,
settings={**settings, "spawn_xy": [210, 30], "ground_z": 1.5},
)
assert terrain_matches(terrain, world)
assert not terrain_matches(terrain, {**world, "collider_sha256": "c" * 64})
assert not terrain_matches(
terrain, {**world, "settings": {**world["settings"], "meters_per_unit": 2}}
)
assert not terrain_matches({**terrain, "generator": "generated-tile"}, world)
def test_square_footprint_fits_straight_corridor_and_rejects_corner_sweep():
check = module("navigation/footprint").swept_footprint_clear
pose = [0, 0, 0.27, 0, 0, 0, 1]
path = [[0, 0, 0], [0.5, 0, 0], [1, 0, 0]]
walls = np.array([[x, y, 0.5, 0.5] for x in np.arange(-1, 2, 0.1) for y in [-0.65, 0.65]])
assert check(path, walls, pose)
walls[:, 1] *= 0.45 / 0.65
assert not check(path, walls, pose)
# A diagonal turn sweeps a square corner into this obstacle, even though
# the chassis at its initial and final straight poses does not contain it.
assert not check([[0, 0, 0], [0.5, 0.5, 0]], [[0.7, 0, 0.5, 0.5]], pose)
def test_command_monitor_covers_deadman_braking_distance_and_rotation():
check = module("navigation/footprint").command_footprint_clear
pose = [0, 0, 0.27, 0, 0, 0, 1]
assert check(0.15, 0, [[1.5, 0, 0.5, 0.5]], pose)
assert not check(0.15, 0, [[0.7, 0, 0.5, 0.5]], pose)
assert not check(0, 0.8, [[0.7, 0, 0.5, 0.5]], pose)
def test_smooth_slope_is_distinct_from_a_step_or_vertical_terrain():
costs = module("navigation/terrain_costs").supported_slope_costs
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)])
slope = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15)))
normalized, corrected = costs(slope)
assert corrected > len(slope) * 0.9
assert normalized[len(slope) // 2, 3] == 0
assert np.array_equal(normalized[:, :3], slope[:, :3])
# A 15 cm ledge across the initial footprint cannot become a traversable ramp.
step = slope.copy()
step[:, 2] = np.where(step[:, 0] >= 0, 0.15, 0)
assert costs(step)[1] == 0
cliff = slope.copy()
cliff[:, 2] = np.where(cliff[:, 0] >= 0, -0.4, 0)
assert costs(cliff)[1] == 0
steep = slope.copy()
steep[:, 2] = steep[:, 0] * np.tan(np.radians(35))
assert costs(steep)[1] == 0
assert costs(slope[np.abs(slope[:, 1]) < 0.01])[1] == 0 # Unobserved lateral support.
def test_underbody_support_does_not_clear_future_terrain_walls_or_drops():
correct = module("navigation/terrain_costs").underbody_support_costs
terrain = np.array(
[
[-0.375, -0.28, 0.066, 0.103], # Low return already under the chassis.
[0.46, 0, 0.066, 0.103], # Inset excludes the leading edge.
[0.75, 0, 0.066, 0.103], # Never change future terrain from body pose.
[0, 0, 0.12, 0.12], # A real step within the footprint remains blocked.
[0, 0, 0.5, 0.5],
[0, 0, -0.4, 0.4],
]
)
original = terrain.copy()
result, count = correct(terrain, [0, 0, 0.37, 0, 0, 0, 1])
assert count == 1 and result[0, 3] == pytest.approx(0.066)
assert np.array_equal(result[1:], original[1:])
assert np.array_equal(terrain, original) # Never erase the causal raw map.
# Rotate both observations and the measured chassis; the result must agree.
yaw = np.pi / 2
rotated = terrain.copy()
rotated[:, :2] = terrain[:, :2] @ np.array([[0, 1], [-1, 0]]) + [3, 4]
pose = [3, 4, 0.37, 0, 0, np.sin(yaw / 2), np.cos(yaw / 2)]
assert np.allclose(correct(rotated, pose)[0][:, 3], result[:, 3])
assert correct(terrain, [0, 0, 0.37, 0, np.sin(np.pi / 12), 0, np.cos(np.pi / 12)])[1] == 0
def test_retreat_requires_observed_full_width_support_and_no_drop_or_step():
choose = module("navigation_client").recovery_goal
points = np.array(
[[x, y, 0.0] for x in np.arange(-1.5, -0.39, 0.05) for y in np.arange(-0.85, 0.86, 0.05)]
)
pose = [0, 0, 0.37, 0, 0, 0, 1]
calibration = {"body_contact_height_m": 0.37}
assert choose(points, pose, calibration) == pytest.approx([-0.65, 0, 0])
assert choose(points[points[:, 1] > -0.2], pose, calibration) is None
assert choose(points[points[:, 0] < -0.9], pose, calibration) is None
for height in (-0.4, 0.15):
discontinuous = points.copy()
discontinuous[points[:, 0] < -0.9, 2] = height
assert choose(discontinuous, pose, calibration) is None
slope = points.copy()
slope[:, 2] = slope[:, 0] * np.tan(np.radians(10))
assert choose(slope, pose, calibration) is not None
assert choose(points, pose, calibration, [-0.65, 0.3, 0]) is None
def test_reverse_monitor_checks_behind_the_body():
check = module("navigation/footprint").command_footprint_clear
pose = [0, 0, 0.37, 0, 0, 0, 1]
assert check(-0.1, 0, [[-1.5, 0, 0.5, 0.5]], pose)
assert not check(-0.1, 0, [[-0.65, 0, 0.5, 0.5]], pose)
def test_legacy_coordinate_migration_is_an_exact_rigid_rotation():
import json
import struct
migrate = module("navigation/migrate_terrain_coordinates")
positions = [[1.0, -2.0, -3.0], [2.0, -2.0, -3.0], [1.0, -1.0, -3.0]]
binary = struct.pack("<9f3I", *(v for p in positions for v in p), 0, 1, 2)
document = {
"nodes": [{"mesh": 0}],
"meshes": [{"primitives": [{"attributes": {"POSITION": 0}, "indices": 1}]}],
"accessors": [
{
"bufferView": 0,
"componentType": 5126,
"type": "VEC3",
"count": 3,
"min": [1, -2, -3],
"max": [2, -1, -3],
},
{"bufferView": 1, "componentType": 5125, "type": "SCALAR", "count": 3},
],
"bufferViews": [{"byteOffset": 0, "byteLength": 36}, {"byteOffset": 36, "byteLength": 12}],
}
raw = json.dumps(document).encode()
raw += b" " * ((-len(raw)) % 4)
glb = (
struct.pack("<III", 0x46546C67, 2, 28 + len(raw) + len(binary))
+ struct.pack("<II", len(raw), 0x4E4F534A)
+ raw
+ struct.pack("<II", len(binary), 0x004E4942)
+ binary
)
corrected = migrate.rotate_glb(glb)
length = struct.unpack_from("<I", corrected, 12)[0]
result = np.array(struct.unpack_from("<9f", corrected, 28 + length)).reshape(-1, 3)
assert np.array_equal(result, np.array(positions) * [-1, 1, -1])
assert np.array_equal(
result[:, [0, 2, 1]] * [1, -1, 1], [[-1, -3, -2], [-2, -3, -2], [-1, -3, -1]]
)
assert struct.unpack_from("<3I", corrected, 28 + length + 36) == (0, 1, 2)
def test_voxel_quantized_grade_does_not_become_a_wall_but_ledge_remains():
costs = module("navigation/terrain_costs").supported_slope_costs
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.06) for y in np.arange(-1, 1.01, 0.06)])
height = np.round(xy[:, 0] * np.tan(np.radians(20)) / 0.06) * 0.06
surface = np.column_stack((xy, height, np.full(len(xy), 0.15)))
assert costs(surface)[1] > len(surface) * 0.8
for discontinuity in (0.12, 0.15, -0.4):
ledge = surface.copy()
ledge[:, 2] = np.where(xy[:, 0] >= 0, discontinuity, 0)
corrected, _ = costs(ledge)
near_edge = np.abs(xy[:, 0]) < 0.12
assert np.all(corrected[near_edge, 3] > 0.1)
stone = surface.copy()
stone[:, 2] = 0
stone[(abs(xy[:, 0]) < 0.12) & (abs(xy[:, 1]) < 0.12), 2] = 0.15
assert costs(stone)[1] == 0
def test_grade_fit_cannot_bridge_an_unobserved_gap():
costs = module("navigation/terrain_costs").supported_slope_costs
points = np.array(
[
[x, y, 0 if x < 0 else 0.15, 0.15]
for x in [-0.3, -0.2, 0.2, 0.3]
for y in np.arange(-0.3, 0.31, 0.1)
]
)
assert costs(points)[1] == 0
def test_existing_reserve_overlap_only_allows_departure_not_approach_or_body_overlap():
check = module("navigation/footprint").command_footprint_clear
pose = [0, 0, 0.37, 0, 0, 0, 1]
behind = [[-0.53, 0, 0.5, 0.5]]
assert check(0.15, 0, behind, pose)
assert not check(-0.1, 0, behind, pose)
assert not check(0, 0.35, behind, pose)
assert not check(0.15, 0, [[-0.49, 0, 0.5, 0.5]], pose)
assert not check(0.15, 0, [[0.53, 0, 0.5, 0.5]], pose)
# A longer admitted camera age must also enlarge the collision envelope.
assert not check(0.15, 0, [[0.80, 0, 0.5, 0.5]], pose)
def test_terrain_fit_cache_invalidates_when_a_new_obstacle_is_observed():
costs = module("navigation/terrain_costs")
normalize = costs.TerrainCostNormalizer()
xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)])
grade = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15)))
clear, count = normalize(grade)
assert count > len(grade) * 0.9
assert np.array_equal(normalize(grade)[0], clear)
changed = np.vstack((grade, [0.02, 0.02, 0.3, 0.3]))
cached, count = normalize(changed)
fresh, expected_count = costs.supported_slope_costs(changed)
assert np.array_equal(cached, fresh) and count == expected_count
assert cached[len(grade) // 2, 3] > 0.1
assert np.array_equal(normalize(grade)[0], clear)
def test_velocity_regulation_keeps_the_selected_arc_and_its_braking_clearance():
monitor = module("navigation/footprint")
pose = [0, 0, 0.37, 0, 0, 0, 1]
hazard = [[0.834, -0.08, 0.15, 0.15]]
speed, yaw, scale = monitor.regulate_command(0.15, -0.245, hazard, pose)
assert 0.2 <= scale < 1
assert speed / yaw == pytest.approx(0.15 / -0.245)
assert monitor.command_footprint_clear(speed, yaw, hazard, pose)
assert monitor.regulate_command(0.15, 0, [[0.56, 0, 0.2, 0.2]], pose) == (0, 0, 0)
assert monitor.regulate_command(0.15, 0, [[0.49, 0, 0.2, 0.2]], pose) == (0, 0, 0)
reverse, _, scale = monitor.regulate_command(-0.1, 0, [[-0.68, 0, 0.2, 0.2]], pose)
assert 0.2 <= scale < 1 and reverse < 0
assert monitor.command_footprint_clear(reverse, 0, [[-0.68, 0, 0.2, 0.2]], pose)
+303
View File
@@ -0,0 +1,303 @@
"""Lifecycle and clock-isolation checks; GPU/stream performance is qualified on Worker."""
import importlib.util
import threading
import time
from pathlib import Path
from types import SimpleNamespace
import numpy as np
import pytest
from pydantic import ValidationError
from test_ai_polygon import make_world, sample
from test_observatory_recorded_jobs import _definitions
from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue
from k1link.simulation.ai_polygon.contracts import (
RealtimeSnapshot,
RunCreate,
StreamEndpoint,
WorkerHello,
)
from k1link.simulation.ai_polygon.runs import RunStore
from k1link.simulation.ai_polygon.worlds import WorldStore
@pytest.fixture
def realtime(tmp_path):
queue = ObservatoryRecordedJobQueue(tmp_path, definitions=_definitions())
store = RunStore(WorldStore(tmp_path), queue)
hello = WorkerHello(
worker_id="worker-006",
instance_id="a" * 32,
runtime="isaac-sim-6.1",
model_ids=["reference"],
profile_sha256="b" * 64,
runtime_sources={key: "c" * 64 for key in ("worker", "scene", "models", "robot")},
execution_modes=["realtime"],
stream=StreamEndpoint(server="100.80.1.2"),
)
store.register(hello)
world = make_world(store.worlds)
row = store.start(
RunCreate(world_id=world["world_id"], clock="realtime", start_paused=True),
"test-realtime-01",
)
return store, hello, row
def snapshot(**changes):
fields = dict(
sequence=0,
control_sequence=0,
state="ready",
phase="running",
simulation_time_ns=0,
wall_elapsed_seconds=1,
physics_steps=0,
render_frames=30,
sensor_frames=0,
inference_count=0,
dropped_frames=0,
rtf=0,
render_fps=30,
sensor_fps=0,
ai_hz=0,
pose_xy=(0, 0),
pose_yaw=0,
speed_mps=0,
applied_speed_mps=0,
applied_yaw_rate_rps=0,
stop_reason="paused",
ai_ready=False,
stream_ready=True,
camera="follow",
)
return RealtimeSnapshot(**{**fields, **changes})
def test_worker_owns_acknowledgement_and_no_frame_archive_on_core(realtime):
store, hello, row = realtime
key = row["run_id"]
assert not (store.directory(key) / "frames").exists()
assert store.poll(hello.instance_id, key)["run"]["state"] == "starting"
store.snapshot(key, hello.instance_id, snapshot())
command = store.control(key, "play")
assert command["state"] == "ready"
assert store.control(key, "play")["control_sequence"] == command["control_sequence"]
assert store.poll(hello.instance_id, key)["run"]["state"] == "ready"
store.snapshot(
key,
hello.instance_id,
snapshot(sequence=1, control_sequence=1, state="running", simulation_time_ns=66666667),
)
assert store.get(key)["state"] == "running"
with pytest.raises(RuntimeError):
store.sample(key, hello.instance_id, sample())
with pytest.raises(RuntimeError):
store.control(key, "step")
def test_disconnect_restart_reconcile_never_releases_gpu_or_replays(realtime):
store, hello, row = realtime
key = row["run_id"]
store.snapshot(key, hello.instance_id, snapshot())
store.seen -= 21
assert store.status()["active_run"]["state"] == "disconnected"
with pytest.raises(RuntimeError):
store.queue.reserve_simulation("airun-" + "d" * 32)
recovered = RunStore(store.worlds, store.queue)
with pytest.raises(RuntimeError):
recovered.register(hello.model_copy(update={"instance_id": "d" * 32}))
recovered.register(hello)
assert recovered.poll(hello.instance_id, key)["action"] == "pause"
recovered.snapshot(key, hello.instance_id, snapshot(sequence=3))
assert recovered.get(key)["state"] == "ready"
recovered.control(key, "stop")
recovered.snapshot(key, hello.instance_id, snapshot(sequence=4))
assert recovered.get(key)["state"] == "stopping"
recovered.finish(key, hello.instance_id, "stopped", "")
recovered.queue.reserve_simulation("airun-" + "d" * 32)
def test_snapshot_is_idempotent_and_rejects_unissued_ack(realtime):
store, hello, row = realtime
key = row["run_id"]
first = snapshot(sequence=5, simulation_time_ns=1000)
assert store.snapshot(key, hello.instance_id, first) == store.snapshot(
key, hello.instance_id, first
)
with pytest.raises(ValueError):
store.snapshot(key, hello.instance_id, snapshot(sequence=6, control_sequence=2))
with pytest.raises(ValueError):
store.snapshot(key, hello.instance_id, snapshot(sequence=6, simulation_time_ns=999))
with pytest.raises(ValidationError):
StreamEndpoint(server="8.8.8.8")
def test_slow_ai_is_bounded_and_pause_fences_inflight_result(tmp_path):
path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
spec = importlib.util.spec_from_file_location("polygon_realtime_ai", path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
entered, release = threading.Event(), threading.Event()
seen = []
class Model:
def ready(self):
pass
def close(self):
pass
def infer(self, rgb):
seen.append(int(rgb[0, 0, 0]))
entered.set()
assert release.wait(2)
return None, []
decision = {
"speed_mps": 0.3,
"yaw_rate_rps": 0,
"reason": "road",
"road_fraction": 0.7,
"obstacle_count": 0,
}
ai = module.LatestInference(
Model,
SimpleNamespace(
reset=lambda: None, decide=lambda *_: SimpleNamespace(model_dump=lambda: decision)
),
tmp_path,
)
ai.start()
try:
ai.enable(True)
ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), 0, time.monotonic(), 0)
assert entered.wait(2)
for frame_id in range(1, 11):
ai.submit(
np.full((2, 2, 3), frame_id, dtype=np.uint8),
frame_id,
time.monotonic(),
frame_id * 1000,
)
assert ai.dropped == 9
assert ai.command(time.monotonic())[0] == 0
ai.enable(False)
release.set()
ai.close()
assert seen == [0]
assert ai.result is None
finally:
release.set()
ai.close()
def test_expired_source_stops_even_if_inference_just_completed(tmp_path):
path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
spec = importlib.util.spec_from_file_location("polygon_command_deadline", path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
ai = module.LatestInference(None, None, tmp_path)
ai.enabled = True
ai.result = {
"captured_at": 0.0,
"completed_at": 1.0,
"decision": {"speed_mps": 0.3, "yaw_rate_rps": 0},
}
assert ai.command(1.1)[:3] == (0.0, 0.0, "stale-camera")
ai.result["captured_at"] = 1.0
assert ai.command(1.1)[:3] == (0.3, 0, "none")
ai.result["decision"]["speed_mps"] = -0.1
assert ai.command(1.1)[:3] == (-0.1, 0, "none")
assert ai.command(1.6)[:2] == (0, 0)
# The worker's 0.8 s camera budget still rejects old images, even when
# a result has just arrived, and never extends the command watchdog.
ai.result.update(captured_at=1.0, completed_at=1.7)
assert ai.command(1.79, frame_deadline=0.8)[:3] == (-0.1, 0, "none")
assert ai.command(1.81, frame_deadline=0.8)[:3] == (0.0, 0.0, "stale-camera")
ai.result.update(captured_at=1.7, completed_at=1.0)
assert ai.command(1.79, frame_deadline=0.8)[:2] == (0.0, 0.0)
def test_reverse_telemetry_keeps_the_run_speed_limit(realtime):
store, hello, row = realtime
store.snapshot(row["run_id"], hello.instance_id, snapshot(applied_speed_mps=-0.1))
assert store.get(row["run_id"])["telemetry"]["applied_speed_mps"] == -0.1
with pytest.raises(ValueError, match="скорость"):
store.snapshot(
row["run_id"], hello.instance_id, snapshot(sequence=1, applied_speed_mps=-0.4)
)
def test_resume_requires_fresh_clear_observations(tmp_path):
from k1link.simulation.ai_polygon.policy import RoadPolicy
path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py"
spec = importlib.util.spec_from_file_location("polygon_resume", path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
class Model:
def ready(self):
pass
def close(self):
pass
def infer(self, rgb):
return np.ones((512, 512), dtype=bool), []
ai = module.LatestInference(Model, RoadPolicy(0.3), tmp_path)
ai.start()
def frame(number):
ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), number, time.monotonic(), number)
deadline = time.monotonic() + 2
while ai.count < number and time.monotonic() < deadline:
time.sleep(0.001)
assert ai.count == number
return ai.command(time.monotonic())[0]
try:
ai.enable(True)
assert frame(1) == frame(2) == 0
assert frame(3) > 0
ai.enable(False)
ai.enable(True)
assert frame(4) == frame(5) == 0
assert frame(6) > 0
finally:
ai.close()
def test_windows_snapshot_sharing_retry_is_bounded(tmp_path, monkeypatch):
path = Path(__file__).parents[1] / "simulation/ai-polygon/local_state.py"
spec = importlib.util.spec_from_file_location("polygon_ipc", path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
original = module.os.replace
attempts = []
def transient(source, target):
attempts.append(1)
if len(attempts) <= 2:
raise PermissionError("Windows sharing violation")
original(source, target)
monkeypatch.setattr(module.os, "replace", transient)
monkeypatch.setattr(module.time, "sleep", lambda _: None)
target = tmp_path / "snapshot.json"
module.write_json(target, {"sequence": 3})
assert module.read_json(target) == {"sequence": 3}
assert len(attempts) == 3
attempts.clear()
def permanent():
attempts.append(1)
raise PermissionError("Not a transient lock")
with pytest.raises(PermissionError):
module.sharing_retry(permanent)
assert len(attempts) == 8
+42
View File
@@ -0,0 +1,42 @@
"""Prevent a reconstructed trunk/face from being admitted between ground rays."""
import importlib.util
from pathlib import Path
import numpy as np
SPEC = importlib.util.spec_from_file_location(
"spawn_clearance", Path(__file__).parents[1] / "simulation/ai-polygon/spawn_clearance.py"
)
MODULE = importlib.util.module_from_spec(SPEC)
SPEC.loader.exec_module(MODULE)
def count(points, xy=(0, 0), heading=0, plane=(0, 0, 0)):
return MODULE.obstructing_triangles(np.array(points), np.array([[0, 1, 2]]), xy, heading, plane)
def test_narrow_trunk_between_support_rays_and_crossing_triangle():
assert count([[0.21, 0.21, 0], [0.23, 0.21, 0], [0.21, 0.21, 2]]) == 1
# All vertices outside the prism, but the face crosses through its centre.
assert count([[-2, 0, 0.3], [2, 0, 0.3], [0, 2, 0.3]]) == 1
def test_ground_step_and_overhead_geometry_do_not_block_start():
for z in (0, 0.05, 0.10, 1.5):
assert count([[-2, -2, z], [2, -2, z], [0, 2, z]]) == 0
assert count([[-2, -2, 0.12], [2, -2, 0.12], [0, 2, 0.12]]) == 1
def test_triangle_bounding_box_alone_does_not_reject_empty_corner():
assert count([[0.4, 2, 0.5], [2, 0.4, 0.5], [2, 2, 0.5]]) == 0
def test_clearance_tracks_translation_heading_and_support_slope():
points = np.array([[0.6, 0, 0.3], [0.7, 0, 0.3], [0.6, 0.03, 0.8]])
assert count(points) == 0
assert count(points, heading=45) == 1
points[:, 2] += points[:, 0] * 0.2 + 4
points[:, :2] += [10, -3]
assert count(points, (10, -3), 45, (0.2, 0, 4)) == 1
assert count([[8, -5, 3.6], [12, -5, 4.4], [10, -1, 4]], (10, -3), 45, (0.2, 0, 4)) == 0
+74
View File
@@ -0,0 +1,74 @@
"""Paired assets remain on Worker; hashes and manifests cross the operator link."""
import hashlib
import importlib
import json
import struct
from pathlib import Path
import numpy as np
import pytest
from test_ai_polygon import ply, worker_asset
def triangle_glb():
binary = np.array([[0, 0, 0], [1, 0, 0], [0, 0, 1]], dtype="<f4").tobytes()
binary += np.array([0, 1, 2], dtype="<u4").tobytes()
doc = {
"buffers": [{"byteLength": 48}],
"bufferViews": [
{"buffer": 0, "byteLength": 36},
{"buffer": 0, "byteOffset": 36, "byteLength": 12},
],
"accessors": [
{"bufferView": 0, "componentType": 5126, "type": "VEC3", "count": 3},
{"bufferView": 1, "componentType": 5125, "type": "SCALAR", "count": 3},
],
"meshes": [{"primitives": [{"attributes": {"POSITION": 0}, "indices": 1}]}],
}
encoded = json.dumps(doc).encode()
encoded += b" " * (-len(encoded) % 4)
return (
struct.pack("<III", 0x46546C67, 2, 28 + len(encoded) + len(binary))
+ struct.pack("<II", len(encoded), 0x4E4F534A)
+ encoded
+ struct.pack("<II", len(binary), 0x004E4942)
+ binary
)
def test_paired_import_checks_actual_files_and_never_fetches_from_operator(tmp_path, monkeypatch):
monkeypatch.syspath_prepend(str(Path(__file__).parents[1] / "simulation/ai-polygon"))
importer = importlib.import_module("register_paired_scene")
preparer = importlib.import_module("prepare_terrain")
client_module = importlib.import_module("core_client")
source, collider = tmp_path / "source.ply", tmp_path / "mesh.glb"
source.write_bytes(ply())
collider.write_bytes(triangle_glb())
descriptor = worker_asset().model_dump(mode="json")
descriptor.update(
sha256=hashlib.sha256(source.read_bytes()).hexdigest(),
collider_sha256=hashlib.sha256(collider.read_bytes()).hexdigest(),
)
root = tmp_path / "worker"
request = importer.stage(root, source, collider, descriptor)
world = {**request.model_dump(mode="json"), "storage": {"kind": "worker"}}
manifest_path = preparer.prepare_terrain(root, tmp_path, world)
assert json.loads(manifest_path.read_text())["collider_sha256"] == descriptor["collider_sha256"]
assert importer.stage(root, source, collider, descriptor) == request
token = tmp_path / "token"
token.write_text("test" * 12)
client = client_module.CoreClient("http://127.0.0.1:18080", token, "a" * 32)
monkeypatch.setattr(
client_module.http.client,
"HTTPConnection",
lambda *args, **kwargs: pytest.fail("Operator must not supply this asset"),
)
cached = root / "state/worlds" / (request.sha256 + ".ply")
client.download(world, cached)
with pytest.raises(RuntimeError, match="missing or changed"):
client.download(world, tmp_path / "missing.ply")
with pytest.raises(ValueError, match="match"):
preparer.prepare_terrain(root, tmp_path, {**world, "collider_sha256": "f" * 64})
with pytest.raises(ValueError, match="identity"):
importer.stage(root, source, collider, {**descriptor, "collider_sha256": "f" * 64})
+44
View File
@@ -1604,3 +1604,47 @@ def test_queue_detects_mutated_immutable_identity(tmp_path: Path) -> None:
with pytest.raises(ObservatoryRecordedQueueIntegrityError, match="identity"): with pytest.raises(ObservatoryRecordedQueueIntegrityError, match="identity"):
queue.get(job.job_id) queue.get(job.job_id)
def test_simulation_reservation_serializes_recorded_claims_and_survives_restart(tmp_path):
queue = _queue(tmp_path)
job, _ = queue.submit(_intent(), enqueue=True)
owner = "airun-" + "a" * 32
queue.reserve_simulation(owner)
queue.reserve_simulation(owner)
restarted = _queue(tmp_path)
assert restarted.claim_next(claimant_id="worker-006", claim_request_id="sim-blocked") is None
assert restarted.get(job.job_id).state == "queued"
with pytest.raises(ObservatoryRecordedQueueConflictError):
restarted.release_simulation("airun-" + "b" * 32)
restarted.release_simulation(owner)
claim = restarted.claim_next(claimant_id="worker-006", claim_request_id="sim-released")
assert claim.job.job_id == job.job_id
with pytest.raises(ObservatoryRecordedQueueBusyError):
queue.reserve_simulation(owner)
def test_simulation_and_recorded_claim_cannot_win_together(tmp_path):
queue = _queue(tmp_path)
queue.submit(_intent(), enqueue=True)
second = _queue(tmp_path)
barrier = Barrier(2)
def simulate():
barrier.wait()
try:
queue.reserve_simulation("airun-" + "a" * 32)
return True
except ObservatoryRecordedQueueBusyError:
return False
def recorded():
barrier.wait()
return (
second.claim_next(claimant_id="worker-006", claim_request_id="racing-claim") is not None
)
with ThreadPoolExecutor(max_workers=2) as pool:
simulation = pool.submit(simulate)
inference = pool.submit(recorded)
assert simulation.result() != inference.result()