feat(simulation): add Worker AI polygon runtime and terrain navigation
This commit is contained in:
@@ -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
|
||||||
@@ -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 }
|
||||||
@@ -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),
|
||||||
|
}
|
||||||
|
)
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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)}))
|
||||||
@@ -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)}))
|
||||||
@@ -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()
|
||||||
@@ -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"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -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))
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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)
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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)
|
||||||
@@ -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;} }
|
||||||
@@ -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
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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
@@ -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" }
|
||||||
|
}
|
||||||
@@ -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,
|
||||||
|
],
|
||||||
|
)
|
||||||
@@ -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)
|
||||||
@@ -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()
|
||||||
@@ -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")
|
||||||
|
|||||||
@@ -1,5 +1,13 @@
|
|||||||
"""Observation-only laboratory orchestration contracts."""
|
"""Observation contracts; platform-specific services are loaded only on demand.
|
||||||
|
|
||||||
|
Importing a portable composition on Windows must not import Core's POSIX
|
||||||
|
artifact gateway or eagerly initialize session/recording infrastructure.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from importlib import import_module
|
||||||
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
from k1link.observatory.canonical_result import (
|
from k1link.observatory.canonical_result import (
|
||||||
is_admitted_observatory_recorded_result,
|
is_admitted_observatory_recorded_result,
|
||||||
)
|
)
|
||||||
@@ -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__))
|
||||||
|
|||||||
@@ -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(
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -1,5 +1,13 @@
|
|||||||
"""Mission Core qualification and simulation boundaries."""
|
"""Mission Core simulation contracts, with platform services loaded on demand.
|
||||||
|
|
||||||
|
Portable AI compositions do not require the legacy S0/YAML or POSIX process
|
||||||
|
supervisor when loaded by the Windows Worker coordinator.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from importlib import import_module
|
||||||
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
from k1link.simulation.contracts import (
|
from k1link.simulation.contracts import (
|
||||||
AckermannControlSetpoint,
|
AckermannControlSetpoint,
|
||||||
AuthorityProfile,
|
AuthorityProfile,
|
||||||
@@ -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,
|
||||||
|
)
|
||||||
@@ -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
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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),
|
||||||
|
|||||||
@@ -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)]
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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)
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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})
|
||||||
@@ -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()
|
||||||
|
|||||||
Reference in New Issue
Block a user