feat(simulation): add Worker AI polygon runtime and terrain navigation
This commit is contained in:
DCCONSTRUCTIONS
@@ -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()
|
||||
Reference in New Issue
Block a user