diff --git a/simulation/ai-polygon/Install-RealtimeWorker.ps1 b/simulation/ai-polygon/Install-RealtimeWorker.ps1 new file mode 100644 index 0000000..37f39ce --- /dev/null +++ b/simulation/ai-polygon/Install-RealtimeWorker.ps1 @@ -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 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') diff --git a/simulation/ai-polygon/Prepare-IsaacRuntime.ps1 b/simulation/ai-polygon/Prepare-IsaacRuntime.ps1 new file mode 100644 index 0000000..ee2b497 --- /dev/null +++ b/simulation/ai-polygon/Prepare-IsaacRuntime.ps1 @@ -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') diff --git a/simulation/ai-polygon/Prepare-RobotAssets.ps1 b/simulation/ai-polygon/Prepare-RobotAssets.ps1 new file mode 100644 index 0000000..badd5b3 --- /dev/null +++ b/simulation/ai-polygon/Prepare-RobotAssets.ps1 @@ -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 diff --git a/simulation/ai-polygon/Prepare-Scene.ps1 b/simulation/ai-polygon/Prepare-Scene.ps1 new file mode 100644 index 0000000..1a12f63 --- /dev/null +++ b/simulation/ai-polygon/Prepare-Scene.ps1 @@ -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 diff --git a/simulation/ai-polygon/Prepare-SplatTools.ps1 b/simulation/ai-polygon/Prepare-SplatTools.ps1 new file mode 100644 index 0000000..439b588 --- /dev/null +++ b/simulation/ai-polygon/Prepare-SplatTools.ps1 @@ -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 } diff --git a/simulation/ai-polygon/build_bundle.py b/simulation/ai-polygon/build_bundle.py new file mode 100644 index 0000000..6de5f60 --- /dev/null +++ b/simulation/ai-polygon/build_bundle.py @@ -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), + } + ) +) diff --git a/simulation/ai-polygon/compose.models.yaml b/simulation/ai-polygon/compose.models.yaml new file mode 100644 index 0000000..f28bfd6 --- /dev/null +++ b/simulation/ai-polygon/compose.models.yaml @@ -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 diff --git a/simulation/ai-polygon/connect-worker.sh b/simulation/ai-polygon/connect-worker.sh new file mode 100644 index 0000000..b7ff5be --- /dev/null +++ b/simulation/ai-polygon/connect-worker.sh @@ -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 diff --git a/simulation/ai-polygon/core_client.py b/simulation/ai-polygon/core_client.py new file mode 100644 index 0000000..b1fb46a --- /dev/null +++ b/simulation/ai-polygon/core_client.py @@ -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() diff --git a/simulation/ai-polygon/ddrnet_server.py b/simulation/ai-polygon/ddrnet_server.py new file mode 100644 index 0000000..46eb2d2 --- /dev/null +++ b/simulation/ai-polygon/ddrnet_server.py @@ -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() diff --git a/simulation/ai-polygon/fetch_lod.py b/simulation/ai-polygon/fetch_lod.py new file mode 100644 index 0000000..3c29e9b --- /dev/null +++ b/simulation/ai-polygon/fetch_lod.py @@ -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)})) diff --git a/simulation/ai-polygon/fetch_sog.py b/simulation/ai-polygon/fetch_sog.py new file mode 100644 index 0000000..80da1f7 --- /dev/null +++ b/simulation/ai-polygon/fetch_sog.py @@ -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)})) diff --git a/simulation/ai-polygon/local_state.py b/simulation/ai-polygon/local_state.py new file mode 100644 index 0000000..de3d3d8 --- /dev/null +++ b/simulation/ai-polygon/local_state.py @@ -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() diff --git a/simulation/ai-polygon/model_stack.py b/simulation/ai-polygon/model_stack.py new file mode 100644 index 0000000..ba01e84 --- /dev/null +++ b/simulation/ai-polygon/model_stack.py @@ -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) diff --git a/simulation/ai-polygon/models.worker-006.json b/simulation/ai-polygon/models.worker-006.json new file mode 100644 index 0000000..16ff244 --- /dev/null +++ b/simulation/ai-polygon/models.worker-006.json @@ -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" + ] +} diff --git a/simulation/ai-polygon/motion_control.py b/simulation/ai-polygon/motion_control.py new file mode 100644 index 0000000..6214702 --- /dev/null +++ b/simulation/ai-polygon/motion_control.py @@ -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 diff --git a/simulation/ai-polygon/navigation/Dockerfile b/simulation/ai-polygon/navigation/Dockerfile new file mode 100644 index 0000000..009969f --- /dev/null +++ b/simulation/ai-polygon/navigation/Dockerfile @@ -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"] diff --git a/simulation/ai-polygon/navigation/Dockerfile.connectivity b/simulation/ai-polygon/navigation/Dockerfile.connectivity new file mode 100644 index 0000000..156b18a --- /dev/null +++ b/simulation/ai-polygon/navigation/Dockerfile.connectivity @@ -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" diff --git a/simulation/ai-polygon/navigation/Dockerfile.footprint b/simulation/ai-polygon/navigation/Dockerfile.footprint new file mode 100644 index 0000000..7a640fa --- /dev/null +++ b/simulation/ai-polygon/navigation/Dockerfile.footprint @@ -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 diff --git a/simulation/ai-polygon/navigation/Prepare-Connectivity.ps1 b/simulation/ai-polygon/navigation/Prepare-Connectivity.ps1 new file mode 100644 index 0000000..d031203 --- /dev/null +++ b/simulation/ai-polygon/navigation/Prepare-Connectivity.ps1 @@ -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 diff --git a/simulation/ai-polygon/navigation/Prepare-Navigation.ps1 b/simulation/ai-polygon/navigation/Prepare-Navigation.ps1 new file mode 100644 index 0000000..bf55ccd --- /dev/null +++ b/simulation/ai-polygon/navigation/Prepare-Navigation.ps1 @@ -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 diff --git a/simulation/ai-polygon/navigation/Prepare-RectangleNavigation.ps1 b/simulation/ai-polygon/navigation/Prepare-RectangleNavigation.ps1 new file mode 100644 index 0000000..a2078e2 --- /dev/null +++ b/simulation/ai-polygon/navigation/Prepare-RectangleNavigation.ps1 @@ -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 diff --git a/simulation/ai-polygon/navigation/Prepare-Terrain.ps1 b/simulation/ai-polygon/navigation/Prepare-Terrain.ps1 new file mode 100644 index 0000000..752cb52 --- /dev/null +++ b/simulation/ai-polygon/navigation/Prepare-Terrain.ps1 @@ -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 diff --git a/simulation/ai-polygon/navigation/fastdds.xml b/simulation/ai-polygon/navigation/fastdds.xml new file mode 100644 index 0000000..eac068e --- /dev/null +++ b/simulation/ai-polygon/navigation/fastdds.xml @@ -0,0 +1,18 @@ + + + + + worker_loopback + UDPv4 + 1048576 + 1048576 +
127.0.0.1
+
+
+ + + worker_loopback + false + + +
diff --git a/simulation/ai-polygon/navigation/footprint.py b/simulation/ai-polygon/navigation/footprint.py new file mode 100644 index 0000000..6a11f42 --- /dev/null +++ b/simulation/ai-polygon/navigation/footprint.py @@ -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 diff --git a/simulation/ai-polygon/navigation/migrate_terrain_coordinates.py b/simulation/ai-polygon/navigation/migrate_terrain_coordinates.py new file mode 100644 index 0000000..59cf350 --- /dev/null +++ b/simulation/ai-polygon/navigation/migrate_terrain_coordinates.py @@ -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(" 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) diff --git a/simulation/ai-polygon/navigation/missioncore_footprint.hpp b/simulation/ai-polygon/navigation/missioncore_footprint.hpp new file mode 100644 index 0000000..66d687a --- /dev/null +++ b/simulation/ai-polygon/navigation/missioncore_footprint.hpp @@ -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 +#include +#include +#include + +template +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> 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; +} diff --git a/simulation/ai-polygon/navigation/patch_footprint.py b/simulation/ai-polygon/navigation/patch_footprint.py new file mode 100644 index 0000000..f142258 --- /dev/null +++ b/simulation/ai-polygon/navigation/patch_footprint.py @@ -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)) diff --git a/simulation/ai-polygon/navigation/qualify.py b/simulation/ai-polygon/navigation/qualify.py new file mode 100644 index 0000000..4b6acb4 --- /dev/null +++ b/simulation/ai-polygon/navigation/qualify.py @@ -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() diff --git a/simulation/ai-polygon/navigation/qualify_connectivity.py b/simulation/ai-polygon/navigation/qualify_connectivity.py new file mode 100644 index 0000000..e6179e3 --- /dev/null +++ b/simulation/ai-polygon/navigation/qualify_connectivity.py @@ -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)})) diff --git a/simulation/ai-polygon/navigation/qualify_ipc.py b/simulation/ai-polygon/navigation/qualify_ipc.py new file mode 100644 index 0000000..c968f41 --- /dev/null +++ b/simulation/ai-polygon/navigation/qualify_ipc.py @@ -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() diff --git a/simulation/ai-polygon/navigation/qualify_perception.py b/simulation/ai-polygon/navigation/qualify_perception.py new file mode 100644 index 0000000..d5c996e --- /dev/null +++ b/simulation/ai-polygon/navigation/qualify_perception.py @@ -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() diff --git a/simulation/ai-polygon/navigation/qualify_placement.py b/simulation/ai-polygon/navigation/qualify_placement.py new file mode 100644 index 0000000..a0a6fea --- /dev/null +++ b/simulation/ai-polygon/navigation/qualify_placement.py @@ -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() diff --git a/simulation/ai-polygon/navigation/replay.py b/simulation/ai-polygon/navigation/replay.py new file mode 100644 index 0000000..0f123cf --- /dev/null +++ b/simulation/ai-polygon/navigation/replay.py @@ -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() diff --git a/simulation/ai-polygon/navigation/server.py b/simulation/ai-polygon/navigation/server.py new file mode 100644 index 0000000..059934a --- /dev/null +++ b/simulation/ai-polygon/navigation/server.py @@ -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() diff --git a/simulation/ai-polygon/navigation/terrain_connectivity.cpp b/simulation/ai-polygon/navigation/terrain_connectivity.cpp new file mode 100644 index 0000000..fabf9ea --- /dev/null +++ b/simulation/ai-polygon/navigation/terrain_connectivity.cpp @@ -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 +#include +template int connected_grade(const T* p, int n) { + if (n <= 0) return 0; + std::vector parent(n); + for(int i=0;iT(0.1001)) return 0; + parent[root(i)]=root(j); + } + } + const int first=root(0); + for(int i=1;i 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) diff --git a/simulation/ai-polygon/navigation_client.py b/simulation/ai-polygon/navigation_client.py new file mode 100644 index 0000000..f0884c0 --- /dev/null +++ b/simulation/ai-polygon/navigation_client.py @@ -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() diff --git a/simulation/ai-polygon/prepare_terrain.py b/simulation/ai-polygon/prepare_terrain.py new file mode 100644 index 0000000..e2cf3b2 --- /dev/null +++ b/simulation/ai-polygon/prepare_terrain.py @@ -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 diff --git a/simulation/ai-polygon/qualify_recovery_sensor.py b/simulation/ai-polygon/qualify_recovery_sensor.py new file mode 100644 index 0000000..3d62f06 --- /dev/null +++ b/simulation/ai-polygon/qualify_recovery_sensor.py @@ -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() diff --git a/simulation/ai-polygon/qualify_rover.py b/simulation/ai-polygon/qualify_rover.py new file mode 100644 index 0000000..43068b4 --- /dev/null +++ b/simulation/ai-polygon/qualify_rover.py @@ -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() diff --git a/simulation/ai-polygon/qualify_runtime.py b/simulation/ai-polygon/qualify_runtime.py new file mode 100644 index 0000000..00376a4 --- /dev/null +++ b/simulation/ai-polygon/qualify_runtime.py @@ -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() diff --git a/simulation/ai-polygon/qualify_scene_contact.py b/simulation/ai-polygon/qualify_scene_contact.py new file mode 100644 index 0000000..5c1d8f2 --- /dev/null +++ b/simulation/ai-polygon/qualify_scene_contact.py @@ -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() diff --git a/simulation/ai-polygon/qualify_terrain_coordinates.py b/simulation/ai-polygon/qualify_terrain_coordinates.py new file mode 100644 index 0000000..c0d2c0a --- /dev/null +++ b/simulation/ai-polygon/qualify_terrain_coordinates.py @@ -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) diff --git a/simulation/ai-polygon/qualify_terrain_drive.py b/simulation/ai-polygon/qualify_terrain_drive.py new file mode 100644 index 0000000..4b6dfca --- /dev/null +++ b/simulation/ai-polygon/qualify_terrain_drive.py @@ -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() diff --git a/simulation/ai-polygon/realtime_ai.py b/simulation/ai-polygon/realtime_ai.py new file mode 100644 index 0000000..eb4a746 --- /dev/null +++ b/simulation/ai-polygon/realtime_ai.py @@ -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) diff --git a/simulation/ai-polygon/realtime_worker.py b/simulation/ai-polygon/realtime_worker.py new file mode 100644 index 0000000..772d5c0 --- /dev/null +++ b/simulation/ai-polygon/realtime_worker.py @@ -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 diff --git a/simulation/ai-polygon/register_paired_scene.py b/simulation/ai-polygon/register_paired_scene.py new file mode 100644 index 0000000..3c96327 --- /dev/null +++ b/simulation/ai-polygon/register_paired_scene.py @@ -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() diff --git a/simulation/ai-polygon/route_author/Dockerfile b/simulation/ai-polygon/route_author/Dockerfile new file mode 100644 index 0000000..7aad2a9 --- /dev/null +++ b/simulation/ai-polygon/route_author/Dockerfile @@ -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"] diff --git a/simulation/ai-polygon/route_author/README.md b/simulation/ai-polygon/route_author/README.md new file mode 100644 index 0000000..996f38e --- /dev/null +++ b/simulation/ai-polygon/route_author/README.md @@ -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. diff --git a/simulation/ai-polygon/route_author/habitat_route.cpp b/simulation/ai-polygon/route_author/habitat_route.cpp new file mode 100644 index 0000000..5458376 --- /dev/null +++ b/simulation/ai-polygon/route_author/habitat_route.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +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(&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(&t),sizeof(t))), "tile header failed"); + require(t.ref && t.size>0 && t.size<100000000, "invalid tile"); + auto* data=static_cast(dtAlloc(t.size,DT_ALLOC_PERM)); + require(data && bool(file.read(reinterpret_cast(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::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 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() diff --git a/simulation/ai-polygon/run_scene.py b/simulation/ai-polygon/run_scene.py new file mode 100644 index 0000000..13c0b0f --- /dev/null +++ b/simulation/ai-polygon/run_scene.py @@ -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() diff --git a/simulation/ai-polygon/segformer/Dockerfile b/simulation/ai-polygon/segformer/Dockerfile new file mode 100644 index 0000000..03193bb --- /dev/null +++ b/simulation/ai-polygon/segformer/Dockerfile @@ -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"] diff --git a/simulation/ai-polygon/segformer/Prepare-Segformer.ps1 b/simulation/ai-polygon/segformer/Prepare-Segformer.ps1 new file mode 100644 index 0000000..be0c5ee --- /dev/null +++ b/simulation/ai-polygon/segformer/Prepare-Segformer.ps1 @@ -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 diff --git a/simulation/ai-polygon/segformer/qualify.py b/simulation/ai-polygon/segformer/qualify.py new file mode 100644 index 0000000..0678ba7 --- /dev/null +++ b/simulation/ai-polygon/segformer/qualify.py @@ -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() diff --git a/simulation/ai-polygon/segformer/server.py b/simulation/ai-polygon/segformer/server.py new file mode 100644 index 0000000..ff1578a --- /dev/null +++ b/simulation/ai-polygon/segformer/server.py @@ -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() diff --git a/simulation/ai-polygon/spawn_clearance.py b/simulation/ai-polygon/spawn_clearance.py new file mode 100644 index 0000000..31a3e0a --- /dev/null +++ b/simulation/ai-polygon/spawn_clearance.py @@ -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()) diff --git a/simulation/ai-polygon/splat-tools/package-lock.json b/simulation/ai-polygon/splat-tools/package-lock.json new file mode 100644 index 0000000..5efc583 --- /dev/null +++ b/simulation/ai-polygon/splat-tools/package-lock.json @@ -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" + } + } + } +} diff --git a/simulation/ai-polygon/splat-tools/package.json b/simulation/ai-polygon/splat-tools/package.json new file mode 100644 index 0000000..8936979 --- /dev/null +++ b/simulation/ai-polygon/splat-tools/package.json @@ -0,0 +1,6 @@ +{ + "name": "ndc-ai-polygon-splat-tools", + "version": "1.0.0", + "private": true, + "dependencies": { "@playcanvas/splat-transform": "3.4.0" } +} diff --git a/simulation/ai-polygon/start_worker.py b/simulation/ai-polygon/start_worker.py new file mode 100644 index 0000000..c3e1e00 --- /dev/null +++ b/simulation/ai-polygon/start_worker.py @@ -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, + ], +) diff --git a/simulation/ai-polygon/terrain.py b/simulation/ai-polygon/terrain.py new file mode 100644 index 0000000..6560515 --- /dev/null +++ b/simulation/ai-polygon/terrain.py @@ -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("= 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) diff --git a/simulation/ai-polygon/worker.py b/simulation/ai-polygon/worker.py new file mode 100644 index 0000000..2ab7bb7 --- /dev/null +++ b/simulation/ai-polygon/worker.py @@ -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() diff --git a/src/k1link/artifacts.py b/src/k1link/artifacts.py index 3a9dab6..9a2e0ce 100644 --- a/src/k1link/artifacts.py +++ b/src/k1link/artifacts.py @@ -1,5 +1,6 @@ from __future__ import annotations +import hashlib import json import os import tempfile @@ -8,6 +9,14 @@ from pathlib import Path from typing import Any +def canonical_json_sha256(value: object) -> str: + """Platform-independent identity for shared Core/Worker JSON contracts.""" + payload = json.dumps( + value, ensure_ascii=False, allow_nan=False, separators=(",", ":"), sort_keys=True + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + def utc_now_iso() -> str: """Return a stable UTC timestamp for manifests and capture artifacts.""" return datetime.now(UTC).isoformat(timespec="milliseconds").replace("+00:00", "Z") diff --git a/src/k1link/observatory/__init__.py b/src/k1link/observatory/__init__.py index 42211f8..766c2dc 100644 --- a/src/k1link/observatory/__init__.py +++ b/src/k1link/observatory/__init__.py @@ -1,33 +1,41 @@ -"""Observation-only laboratory orchestration contracts.""" +"""Observation contracts; platform-specific services are loaded only on demand. -from k1link.observatory.canonical_result import ( - is_admitted_observatory_recorded_result, -) -from k1link.observatory.run_preparations import ( - MAX_RUN_PREPARATION_RECORDS, - MAX_RUN_PREPARATION_STORAGE_BYTES, - OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA, - OBSERVATORY_RUN_PREPARATION_SCHEMA, - RUN_PREPARATION_DATABASE_NAME, - RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS, - ObservatoryRunPreparation, - ObservatoryRunPreparationCapacityError, - ObservatoryRunPreparationConflictError, - ObservatoryRunPreparationError, - ObservatoryRunPreparationIntegrityError, - ObservatoryRunPreparationIntent, - ObservatoryRunPreparationLedger, - ObservatoryRunPreparationNotFoundError, - load_observatory_run_preparation_ledger, - observatory_run_preparation_request_sha256, -) -from k1link.observatory.setups import ( - LABORATORY_SETUP_CATALOG_SCHEMA, - LABORATORY_SETUP_REGISTRY_SCHEMA, - OBSERVATORY_CALCULATION_PROFILE_SCHEMA, - LaboratorySetupRegistry, - LaboratorySetupRegistryError, -) +Importing a portable composition on Windows must not import Core's POSIX +artifact gateway or eagerly initialize session/recording infrastructure. +""" + +from importlib import import_module +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from k1link.observatory.canonical_result import ( + is_admitted_observatory_recorded_result, + ) + from k1link.observatory.run_preparations import ( + MAX_RUN_PREPARATION_RECORDS, + MAX_RUN_PREPARATION_STORAGE_BYTES, + OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA, + OBSERVATORY_RUN_PREPARATION_SCHEMA, + RUN_PREPARATION_DATABASE_NAME, + RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS, + ObservatoryRunPreparation, + ObservatoryRunPreparationCapacityError, + ObservatoryRunPreparationConflictError, + ObservatoryRunPreparationError, + ObservatoryRunPreparationIntegrityError, + ObservatoryRunPreparationIntent, + ObservatoryRunPreparationLedger, + ObservatoryRunPreparationNotFoundError, + load_observatory_run_preparation_ledger, + observatory_run_preparation_request_sha256, + ) + from k1link.observatory.setups import ( + LABORATORY_SETUP_CATALOG_SCHEMA, + LABORATORY_SETUP_REGISTRY_SCHEMA, + OBSERVATORY_CALCULATION_PROFILE_SCHEMA, + LaboratorySetupRegistry, + LaboratorySetupRegistryError, + ) __all__ = [ "LABORATORY_SETUP_CATALOG_SCHEMA", @@ -53,3 +61,41 @@ __all__ = [ "load_observatory_run_preparation_ledger", "observatory_run_preparation_request_sha256", ] + +_EXPORTS = { + "is_admitted_observatory_recorded_result": "k1link.observatory.canonical_result", + "MAX_RUN_PREPARATION_RECORDS": "k1link.observatory.run_preparations", + "MAX_RUN_PREPARATION_STORAGE_BYTES": "k1link.observatory.run_preparations", + "OBSERVATORY_RUN_PREPARATION_REQUEST_SCHEMA": "k1link.observatory.run_preparations", + "OBSERVATORY_RUN_PREPARATION_SCHEMA": "k1link.observatory.run_preparations", + "RUN_PREPARATION_DATABASE_NAME": "k1link.observatory.run_preparations", + "RUN_PREPARATION_SQLITE_LOCK_TIMEOUT_SECONDS": "k1link.observatory.run_preparations", + "ObservatoryRunPreparation": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationCapacityError": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationConflictError": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationError": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationIntegrityError": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationIntent": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationLedger": "k1link.observatory.run_preparations", + "ObservatoryRunPreparationNotFoundError": "k1link.observatory.run_preparations", + "load_observatory_run_preparation_ledger": "k1link.observatory.run_preparations", + "observatory_run_preparation_request_sha256": "k1link.observatory.run_preparations", + "LABORATORY_SETUP_CATALOG_SCHEMA": "k1link.observatory.setups", + "LABORATORY_SETUP_REGISTRY_SCHEMA": "k1link.observatory.setups", + "OBSERVATORY_CALCULATION_PROFILE_SCHEMA": "k1link.observatory.setups", + "LaboratorySetupRegistry": "k1link.observatory.setups", + "LaboratorySetupRegistryError": "k1link.observatory.setups", +} + + +def __getattr__(name: str): + module = _EXPORTS.get(name) + if module is None: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + value = getattr(import_module(module), name) + globals()[name] = value + return value + + +def __dir__(): + return sorted(set(globals()) | set(__all__)) diff --git a/src/k1link/observatory/modular_composition.py b/src/k1link/observatory/modular_composition.py index b71465a..1241211 100644 --- a/src/k1link/observatory/modular_composition.py +++ b/src/k1link/observatory/modular_composition.py @@ -12,7 +12,7 @@ from dataclasses import dataclass from pathlib import Path from typing import Final, cast -from k1link.observatory.portable_run_definitions import canonical_sha256 +from k1link.artifacts import canonical_json_sha256 as canonical_sha256 COMPOSITION_SCHEMA: Final = "missioncore.observatory-ai-composition/v1" MODULE_SCHEMA: Final = "missioncore.observatory-ai-module/v1" @@ -179,6 +179,11 @@ class CompositionSpec: """Source-independent graph, in deterministic topological execution order.""" nodes: tuple[CompositionNode, ...] + execution_mode: str = "recorded-observation-only" + + def __post_init__(self) -> None: + if self.execution_mode not in ("recorded-observation-only", "worker-local-simulation"): + raise CompositionError("unsupported composition execution mode") @property def source_capabilities(self) -> tuple[str, ...]: @@ -212,7 +217,10 @@ class CompositionSpec: "nodes": [node.as_dict() for node in self.nodes], "source_capabilities": list(self.source_capabilities), "outputs": list(self.outputs), - "execution": {"max_parallel_nodes": 1, "mode": "recorded-observation-only"}, + "execution": { + "max_parallel_nodes": 2 if self.execution_mode == "worker-local-simulation" else 1, + "mode": self.execution_mode, + }, } @property @@ -286,7 +294,9 @@ class ModuleRegistry: ], } - def compose(self, document: object) -> CompositionSpec: + def compose( + self, document: object, *, execution_mode: str = "recorded-observation-only" + ) -> CompositionSpec: root = _object(document, {"schema_version", "selections"}) if root["schema_version"] != COMPOSITION_SCHEMA: raise CompositionError("unsupported composition schema") @@ -381,7 +391,7 @@ class ModuleRegistry: for key in ready: ordered.append(pending.pop(key)) emitted.add(key) - return CompositionSpec(tuple(ordered)) + return CompositionSpec(tuple(ordered), execution_mode=execution_mode) def node_input_identity( diff --git a/src/k1link/observatory/recorded_jobs.py b/src/k1link/observatory/recorded_jobs.py index b8a3fbb..c863b3a 100644 --- a/src/k1link/observatory/recorded_jobs.py +++ b/src/k1link/observatory/recorded_jobs.py @@ -97,6 +97,11 @@ _AUTHORITY: Final = { } _SCHEMA_SQL = """ +CREATE TABLE IF NOT EXISTS simulation_worker_reservation ( + singleton INTEGER PRIMARY KEY CHECK (singleton = 1), + owner_id TEXT NOT NULL, + created_at_utc TEXT NOT NULL +); CREATE TABLE IF NOT EXISTS observatory_recorded_jobs ( job_id TEXT PRIMARY KEY, idempotency_key TEXT NOT NULL UNIQUE, @@ -1185,6 +1190,46 @@ class ObservatoryRecordedJobQueue: self._lock = threading.RLock() self._initialize() + def reserve_simulation(self, owner_id: str) -> None: + """Reserve this Worker's GPU atomically against recorded/live admission. + + A lost heartbeat does not prove GPU release. This reservation survives + restarts and is removed only by the exact simulator's release receipt. + """ + if not re.fullmatch(r"airun-[a-f0-9]{32}", owner_id): + raise ValueError("invalid simulation resource owner") + with self._transaction() as connection: + existing = connection.execute( + "SELECT owner_id FROM simulation_worker_reservation" + ).fetchone() + if existing is not None: + if existing["owner_id"] == owner_id: + return + raise ObservatoryRecordedQueueBusyError("Worker занят другим прогоном симуляции.") + busy = connection.execute( + "SELECT job_id FROM observatory_recorded_jobs " + "WHERE state IN ('claimed', 'running', 'paused', 'preemption-pending', " + "'reconciliation-required') LIMIT 1" + ).fetchone() + if busy is not None or self._open_live_lease_row(connection) is not None: + raise ObservatoryRecordedQueueBusyError("Worker занят задачей AI Inference.") + connection.execute( + "INSERT INTO simulation_worker_reservation VALUES (1, ?, ?)", + (owner_id, self._timestamp()), + ) + + def release_simulation(self, owner_id: str) -> None: + """Called only after the trusted simulator reports all GPU work stopped.""" + with self._transaction() as connection: + existing = connection.execute( + "SELECT owner_id FROM simulation_worker_reservation" + ).fetchone() + if existing is not None and existing["owner_id"] != owner_id: + raise ObservatoryRecordedQueueConflictError("simulation resource owner changed") + connection.execute( + "DELETE FROM simulation_worker_reservation WHERE owner_id = ?", (owner_id,) + ) + def resolve_definition( self, setup_id: str, @@ -1444,7 +1489,10 @@ class ObservatoryRecordedJobQueue: label="legacy claim receipt", ) row = None - if self._open_live_lease_row(connection) is None: + simulation = connection.execute( + "SELECT owner_id FROM simulation_worker_reservation" + ).fetchone() + if self._open_live_lease_row(connection) is None and simulation is None: active_owner = connection.execute( "SELECT job_id FROM observatory_recorded_jobs " "WHERE state IN ('claimed', 'running', 'preemption-pending', " @@ -2382,6 +2430,10 @@ class ObservatoryRecordedJobQueue: _validate_pattern(lease_id, _LEASE_ID, "live lease id") self.recover_stale_claims() with self._transaction() as connection: + if connection.execute("SELECT owner_id FROM simulation_worker_reservation").fetchone(): + raise ObservatoryRecordedQueueBusyError( + "simulation has not released the Worker GPU" + ) lease = self._get_live_lease(connection, lease_id) if lease.state == "active": return lease diff --git a/src/k1link/simulation/__init__.py b/src/k1link/simulation/__init__.py index 328c93b..516c319 100644 --- a/src/k1link/simulation/__init__.py +++ b/src/k1link/simulation/__init__.py @@ -1,78 +1,86 @@ -"""Mission Core qualification and simulation boundaries.""" +"""Mission Core simulation contracts, with platform services loaded on demand. -from k1link.simulation.contracts import ( - AckermannControlSetpoint, - AuthorityProfile, - CommandAuthorityScope, - ControlProfile, - ControlSetpoint, - DifferentialControlSetpoint, - ProviderPin, - QualificationArtifact, - QualificationEvent, - QualificationRun, - ReproducibilityTier, - RunKind, - RunState, - SimulationContractError, -) -from k1link.simulation.orchestrator import ( - ActiveQualificationRunError, - SimulationApplicationService, - SimulationOrchestratorError, - SimulationWorkerPort, - WorkerStartResult, - WorkerStopResult, -) -from k1link.simulation.process_supervisor import ( - OwnedProcess, - PosixProcessSupervisor, - ProcessSpec, - ProcessStopResult, - ProcessSupervisorError, -) -from k1link.simulation.provider_contract import ( - PROVIDER_PROFILE_SCHEMA, - ProviderRole, - SimulationClockDescriptor, - SimulationProviderContractError, - SimulationProviderDescriptor, - SimulationProviderProfile, -) -from k1link.simulation.run_store import ( - QualificationRunConflictError, - QualificationRunIntegrityError, - QualificationRunNotFoundError, - QualificationRunStore, - QualificationRunStoreError, - QualificationRunTransitionError, -) -from k1link.simulation.s0 import ( - CheckStatus, - DoctorVerdict, - RuntimeAcceptance, - S0DoctorReport, - S0Profile, - S0ProfileError, - load_s0_profile, - run_s0_doctor, -) -from k1link.simulation.stock_rover import ( - LIFECYCLE_PROFILE_SCHEMA, - StockRoverLifecycleProfile, - StockRoverProfileError, - StockRoverTargetPaths, - load_stock_rover_lifecycle_profile, - stock_rover_process_environment, - stock_rover_process_specs, -) -from k1link.simulation.worker import ( - LocalProcessWorkerAdapter, - S0WorkerGuard, - SimulationWorldControl, - WorkerAdmission, - WorkerAdmissionError, -) +Portable AI compositions do not require the legacy S0/YAML or POSIX process +supervisor when loaded by the Windows Worker coordinator. +""" + +from importlib import import_module +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from k1link.simulation.contracts import ( + AckermannControlSetpoint, + AuthorityProfile, + CommandAuthorityScope, + ControlProfile, + ControlSetpoint, + DifferentialControlSetpoint, + ProviderPin, + QualificationArtifact, + QualificationEvent, + QualificationRun, + ReproducibilityTier, + RunKind, + RunState, + SimulationContractError, + ) + from k1link.simulation.orchestrator import ( + ActiveQualificationRunError, + SimulationApplicationService, + SimulationOrchestratorError, + SimulationWorkerPort, + WorkerStartResult, + WorkerStopResult, + ) + from k1link.simulation.process_supervisor import ( + OwnedProcess, + PosixProcessSupervisor, + ProcessSpec, + ProcessStopResult, + ProcessSupervisorError, + ) + from k1link.simulation.provider_contract import ( + PROVIDER_PROFILE_SCHEMA, + ProviderRole, + SimulationClockDescriptor, + SimulationProviderContractError, + SimulationProviderDescriptor, + SimulationProviderProfile, + ) + from k1link.simulation.run_store import ( + QualificationRunConflictError, + QualificationRunIntegrityError, + QualificationRunNotFoundError, + QualificationRunStore, + QualificationRunStoreError, + QualificationRunTransitionError, + ) + from k1link.simulation.s0 import ( + CheckStatus, + DoctorVerdict, + RuntimeAcceptance, + S0DoctorReport, + S0Profile, + S0ProfileError, + load_s0_profile, + run_s0_doctor, + ) + from k1link.simulation.stock_rover import ( + LIFECYCLE_PROFILE_SCHEMA, + StockRoverLifecycleProfile, + StockRoverProfileError, + StockRoverTargetPaths, + load_stock_rover_lifecycle_profile, + stock_rover_process_environment, + stock_rover_process_specs, + ) + from k1link.simulation.worker import ( + LocalProcessWorkerAdapter, + S0WorkerGuard, + SimulationWorldControl, + WorkerAdmission, + WorkerAdmissionError, + ) __all__ = [ "AckermannControlSetpoint", @@ -133,3 +141,76 @@ __all__ = [ "stock_rover_process_environment", "stock_rover_process_specs", ] + +_EXPORTS = { + "AckermannControlSetpoint": "k1link.simulation.contracts", + "AuthorityProfile": "k1link.simulation.contracts", + "CommandAuthorityScope": "k1link.simulation.contracts", + "ControlProfile": "k1link.simulation.contracts", + "ControlSetpoint": "k1link.simulation.contracts", + "DifferentialControlSetpoint": "k1link.simulation.contracts", + "ProviderPin": "k1link.simulation.contracts", + "QualificationArtifact": "k1link.simulation.contracts", + "QualificationEvent": "k1link.simulation.contracts", + "QualificationRun": "k1link.simulation.contracts", + "ReproducibilityTier": "k1link.simulation.contracts", + "RunKind": "k1link.simulation.contracts", + "RunState": "k1link.simulation.contracts", + "SimulationContractError": "k1link.simulation.contracts", + "ActiveQualificationRunError": "k1link.simulation.orchestrator", + "SimulationApplicationService": "k1link.simulation.orchestrator", + "SimulationOrchestratorError": "k1link.simulation.orchestrator", + "SimulationWorkerPort": "k1link.simulation.orchestrator", + "WorkerStartResult": "k1link.simulation.orchestrator", + "WorkerStopResult": "k1link.simulation.orchestrator", + "OwnedProcess": "k1link.simulation.process_supervisor", + "PosixProcessSupervisor": "k1link.simulation.process_supervisor", + "ProcessSpec": "k1link.simulation.process_supervisor", + "ProcessStopResult": "k1link.simulation.process_supervisor", + "ProcessSupervisorError": "k1link.simulation.process_supervisor", + "PROVIDER_PROFILE_SCHEMA": "k1link.simulation.provider_contract", + "ProviderRole": "k1link.simulation.provider_contract", + "SimulationClockDescriptor": "k1link.simulation.provider_contract", + "SimulationProviderContractError": "k1link.simulation.provider_contract", + "SimulationProviderDescriptor": "k1link.simulation.provider_contract", + "SimulationProviderProfile": "k1link.simulation.provider_contract", + "QualificationRunConflictError": "k1link.simulation.run_store", + "QualificationRunIntegrityError": "k1link.simulation.run_store", + "QualificationRunNotFoundError": "k1link.simulation.run_store", + "QualificationRunStore": "k1link.simulation.run_store", + "QualificationRunStoreError": "k1link.simulation.run_store", + "QualificationRunTransitionError": "k1link.simulation.run_store", + "CheckStatus": "k1link.simulation.s0", + "DoctorVerdict": "k1link.simulation.s0", + "RuntimeAcceptance": "k1link.simulation.s0", + "S0DoctorReport": "k1link.simulation.s0", + "S0Profile": "k1link.simulation.s0", + "S0ProfileError": "k1link.simulation.s0", + "load_s0_profile": "k1link.simulation.s0", + "run_s0_doctor": "k1link.simulation.s0", + "LIFECYCLE_PROFILE_SCHEMA": "k1link.simulation.stock_rover", + "StockRoverLifecycleProfile": "k1link.simulation.stock_rover", + "StockRoverProfileError": "k1link.simulation.stock_rover", + "StockRoverTargetPaths": "k1link.simulation.stock_rover", + "load_stock_rover_lifecycle_profile": "k1link.simulation.stock_rover", + "stock_rover_process_environment": "k1link.simulation.stock_rover", + "stock_rover_process_specs": "k1link.simulation.stock_rover", + "LocalProcessWorkerAdapter": "k1link.simulation.worker", + "S0WorkerGuard": "k1link.simulation.worker", + "SimulationWorldControl": "k1link.simulation.worker", + "WorkerAdmission": "k1link.simulation.worker", + "WorkerAdmissionError": "k1link.simulation.worker", +} + + +def __getattr__(name: str): + module = _EXPORTS.get(name) + if module is None: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + value = getattr(import_module(module), name) + globals()[name] = value + return value + + +def __dir__(): + return sorted(set(globals()) | set(__all__)) diff --git a/src/k1link/simulation/ai_polygon/__init__.py b/src/k1link/simulation/ai_polygon/__init__.py new file mode 100644 index 0000000..2d948d5 --- /dev/null +++ b/src/k1link/simulation/ai_polygon/__init__.py @@ -0,0 +1 @@ +"""Independent, virtual-only AI polygon; never routes assets through the LCC pipeline.""" diff --git a/src/k1link/simulation/ai_polygon/composition.py b/src/k1link/simulation/ai_polygon/composition.py new file mode 100644 index 0000000..e3f3d2e --- /dev/null +++ b/src/k1link/simulation/ai_polygon/composition.py @@ -0,0 +1,194 @@ +"""Simulation providers use the same immutable typed composition as AI Inference. + +Pinhole camera contracts are intentionally distinct from recorded K1/KB4 ports. +Only installed adapters are executable; selection JSON never carries code. +""" + +import hashlib +import json +from pathlib import Path + +from k1link.observatory.modular_composition import ( + COMPOSITION_SCHEMA, + CompositionError, + ModuleRegistry, + ModuleSpec, + canonical_bytes, +) + + +def digest(value): + return hashlib.sha256(canonical_bytes(value)).hexdigest() + + +def registry(adapter_root: Path) -> ModuleRegistry: + profile = json.loads((adapter_root / "models.worker-006.json").read_text()) + models = {m["id"]: m for m in profile["models"]} + implementation = hashlib.sha256( + Path(__file__).with_name("inference.py").read_bytes() + ).hexdigest() + ddr, detector = models["ddrnet-goose-pytorch-reference"], models["rf_detr_large"] + nav = profile["navigation"] + ade = models["segformer-b2-ade150"] + return ModuleRegistry( + ( + ModuleSpec( + "simulation-ddrnet-goose", + "DDRNet · GOOSE 64", + "segmentation", + ddr["image"].removeprefix("sha256:"), + implementation, + ddr["checkpoint_sha256"], + digest( + { + "camera": profile["camera"], + "preprocess": ddr["preprocess"], + "labels": profile["labels_sha256"], + } + ), + ("source.camera.rgb",), + ("segmentation.labels", "segmentation.surface"), + ), + ModuleSpec( + "simulation-segformer-ade", + "SegFormer · природные поверхности", + "segmentation", + ade["image"].removeprefix("sha256:"), + digest( + { + "client": implementation, + "server": hashlib.sha256( + (adapter_root / "segformer/server.py").read_bytes() + ).hexdigest(), + } + ), + ade["checkpoint_sha256"], + digest( + { + "camera": profile["camera"], + "preprocess": ade["preprocess"], + "config": ade["config_sha256"], + "processor": ade["processor_sha256"], + "output": "ade150-labels-and-bool-surface-candidate-square512-v1", + } + ), + ("source.camera.rgb",), + ("segmentation.labels", "segmentation.surface"), + ), + ModuleSpec( + "simulation-rf-detr", + "RF-DETR · люди и животные", + "detection", + detector["image"].removeprefix("sha256:"), + implementation, + detector["checkpoint_sha256"], + digest( + { + "camera": profile["camera"], + "preprocess": detector["preprocess"], + "output": "normalized-xyxy-risk-boxes-v1", + } + ), + ("source.camera.rgb",), + ("detection.boxes",), + ), + ModuleSpec( + "simulation-waypoint-mission", + "Маршрут · контроль продвижения", + "policy", + nav["image"].removeprefix("sha256:"), + digest( + { + "policy": hashlib.sha256( + Path(__file__).with_name("mission_policy.py").read_bytes() + ).hexdigest(), + "adapter": hashlib.sha256( + (adapter_root / "navigation_client.py").read_bytes() + ).hexdigest(), + } + ), + None, + digest( + { + "task": "operator-metric-waypoints-v1", + "recovery": "three-observed-reverse-and-replan-attempts-v2", + } + ), + ( + "segmentation.surface", + "source.camera.calibration", + "source.lidar", + "source.pose", + "source.simulation-time", + ), + ("navigation.goal", "navigation.intent"), + state_policy="causal-reset-at-source-start", + ), + ModuleSpec( + "simulation-cmu-navigation", + "CMU · рельеф и движение", + "motion", + nav["image"].removeprefix("sha256:"), + digest( + { + p.relative_to(adapter_root).as_posix(): hashlib.sha256( + p.read_bytes() + ).hexdigest() + for p in ( + adapter_root / "navigation/server.py", + adapter_root / "navigation/footprint.py", + adapter_root / "navigation/terrain_costs.py", + adapter_root / "navigation/terrain_connectivity.cpp", + adapter_root / "navigation/fastdds.xml", + adapter_root / "navigation_client.py", + ) + } + ), + None, + digest( + { + "upstream": nav["upstream_commit"], + "footprint": [1, 1], + "inputs": "metric-occluded-range-and-rgb", + "command": "signed-mps-rps-observed-recovery-v2", + } + ), + ( + "detection.boxes", + "navigation.goal", + "navigation.intent", + "segmentation.surface", + "source.camera.calibration", + "source.lidar", + "source.pose", + ), + ("motion.command", "motion.path"), + state_policy="causal-reset-at-source-start", + ), + ) + ) + + +def compose(adapter_root: Path, selection=None): + installed = registry(adapter_root) + if selection is None: + defaults = json.loads((adapter_root / "models.worker-006.json").read_text())[ + "default_modules" + ] + selection = { + "schema_version": COMPOSITION_SCHEMA, + "selections": [ + { + "group": m.group, + "module_id": m.module_id, + "module_sha256": m.sha256, + "parameters": {}, + } + for m in installed.modules + if m.module_id in defaults + ], + } + result = installed.compose(selection, execution_mode="worker-local-simulation") + if "motion.command" not in result.outputs: + raise CompositionError("Для движения выберите модуль навигации и его зависимости.") + return result diff --git a/src/k1link/simulation/ai_polygon/contracts.py b/src/k1link/simulation/ai_polygon/contracts.py new file mode 100644 index 0000000..c3b8a11 --- /dev/null +++ b/src/k1link/simulation/ai_polygon/contracts.py @@ -0,0 +1,194 @@ +"""Versioned scene preparation and camera-driven run contracts.""" + +from ipaddress import ip_address, ip_network +from typing import Annotated, Literal + +from pydantic import BaseModel, ConfigDict, Field, HttpUrl, field_validator + + +class Contract(BaseModel): + model_config = ConfigDict(extra="forbid", allow_inf_nan=False, str_strip_whitespace=True) + + +class WorldCreate(Contract): + name: str = Field(min_length=1, max_length=120) + filename: str = Field(pattern=r"^[^/\\\x00-\x1f]{1,200}\.[pP][lL][yY]$") + byte_length: int = Field(gt=0, le=8 * 1024**3, strict=True) + author: str = Field(min_length=1, max_length=160) + license: str = Field(min_length=1, max_length=160) + source_url: HttpUrl | None = None + + +class WorldSettings(Contract): + # Source -> metric Z-up world; collision proxy is prepared separately. + meters_per_unit: float = Field(default=1, ge=0.0001, le=1000) + rotation_degrees: tuple[ + Annotated[float, Field(ge=-360, le=360)], + Annotated[float, Field(ge=-360, le=360)], + Annotated[float, Field(ge=-360, le=360)], + ] = (0, 0, 0) + ground_z: float = Field(default=0, ge=-10000, le=10000) + spawn_xy: tuple[ + Annotated[float, Field(ge=-10000, le=10000)], Annotated[float, Field(ge=-10000, le=10000)] + ] = (0, 0) + heading_degrees: float = Field(default=0, ge=-360, le=360) + camera_height_m: float = Field(default=0.5, ge=0.1, le=3) + max_speed_mps: float = Field(default=0.3, ge=0.05, le=1) + prepared: bool = False + route_xy: list[ + tuple[ + Annotated[float, Field(ge=-10000, le=10000)], + Annotated[float, Field(ge=-10000, le=10000)], + ] + ] = Field(default_factory=list, max_length=32) + + +class RunCreate(Contract): + world_id: str = Field(pattern=r"^aiworld-[a-f0-9]{32}$") + max_steps: int = Field(default=600, ge=1, le=3600, strict=True) + clock: Literal["lockstep", "realtime"] = "lockstep" + start_paused: bool = False + duration_seconds: int = Field(default=1800, ge=10, le=7200, strict=True) + composition: dict | None = None + + +class WorkerWorldCreate(WorldCreate): + """Attestation from the connected Worker after validating its local files.""" + + sha256: str = Field(pattern=r"^[a-f0-9]{64}$") + collider_sha256: str = Field(pattern=r"^[a-f0-9]{64}$") + splat_count: int = Field(ge=1, le=20_000_000, strict=True) + settings: WorldSettings + + +class StreamEndpoint(Contract): + server: str + signaling_port: Literal[49100] = 49100 + media_port: Literal[47998] = 47998 + width: Literal[1280] = 1280 + height: Literal[720] = 720 + fps: Literal[30] = 30 + + @field_validator("server") + @classmethod + def private_address(cls, value: str) -> str: + address = ip_address(value) + if ( + address.version != 4 + or not (address.is_private or address in ip_network("100.64.0.0/10")) + or address.is_unspecified + or address.is_multicast + ): + raise ValueError("Streaming requires a private IPv4 address") + return str(address) + + +class WorkerHello(Contract): + worker_id: str = Field(pattern=r"^[a-zA-Z0-9_-]{1,80}$") + instance_id: str = Field(pattern=r"^[a-f0-9]{32}$") + runtime: Literal["isaac-sim-6.1"] + model_ids: list[Annotated[str, Field(min_length=1, max_length=100)]] = Field( + min_length=1, max_length=8 + ) + runtime_sources: dict[ + Literal["worker", "scene", "models", "robot"], + Annotated[str, Field(pattern=r"^[a-f0-9]{64}$")], + ] = Field(min_length=4, max_length=4) + profile_sha256: str = Field(pattern=r"^[a-f0-9]{64}$") + execution_modes: list[Literal["lockstep", "realtime"]] = ["lockstep"] + stream: StreamEndpoint | None = None + + +class Decision(Contract): + speed_mps: float = Field(ge=-1, le=1) + yaw_rate_rps: float = Field(ge=-1, le=1) + reason: Literal[ + "road", + "obstacle", + "no-road", + "uncertain", + "inference-error", + "replanning", + "stuck", + "goal-reached", + "unstable", + "waiting", + ] + road_fraction: float = Field(ge=0, le=1) + obstacle_count: int = Field(ge=0, le=300, strict=True) + + +class RunSample(Contract): + sequence: int = Field(ge=0, le=3600, strict=True) + simulation_time_ns: int = Field(ge=0, strict=True) + inference_ms: float = Field(ge=0, le=120000) + # The snapshot is an observation BEFORE the action below is applied. + pose_xy: tuple[float, float] + decision: Decision + image_jpeg_base64: str = Field(max_length=2 * 1024 * 1024) + + +class WorkerPoll(Contract): + instance_id: str = Field(pattern=r"^[a-f0-9]{32}$") + run_id: str | None = Field(default=None, pattern=r"^airun-[a-f0-9]{32}$") + + +class RunProgress(Contract): + phase: Literal["world", "models", "scene"] + + +class RunApplied(Contract): + sequence: int = Field(ge=0, le=3600, strict=True) + simulation_time_ns: int = Field(ge=0, strict=True) + physics_steps: Literal[6] + pose_xy: tuple[float, float] + pose_yaw: float | None = None + cycle_ms: float | None = Field(default=None, ge=0, le=120000) + render_ms: float | None = Field(default=None, ge=0, le=120000) + transport_ms: float | None = Field(default=None, ge=0, le=120000) + + +class WorkerResult(Contract): + instance_id: str = Field(pattern=r"^[a-f0-9]{32}$") + outcome: Literal["completed", "stopped", "failed"] + message: str = Field(default="", max_length=500) + resources_released: Literal[True] + + +class ViewControl(Contract): + camera: Literal["follow", "overview", "camera"] + + +class RealtimeSnapshot(Contract): + """Bounded observation of Worker-owned state; never a physics-step receipt.""" + + sequence: int = Field(ge=0, strict=True) + control_sequence: int = Field(ge=0, strict=True) + state: Literal["ready", "running", "paused", "stopping"] + phase: Literal["scene", "models", "running"] + simulation_time_ns: int = Field(ge=0, strict=True) + wall_elapsed_seconds: float = Field(ge=0) + physics_steps: int = Field(ge=0, strict=True) + render_frames: int = Field(ge=0, strict=True) + sensor_frames: int = Field(ge=0, strict=True) + inference_count: int = Field(ge=0, strict=True) + dropped_frames: int = Field(ge=0, strict=True) + rtf: float = Field(ge=0, le=100) + render_fps: float = Field(ge=0, le=1000) + sensor_fps: float = Field(ge=0, le=1000) + ai_hz: float = Field(ge=0, le=1000) + inference_ms: float | None = Field(default=None, ge=0) + frame_age_ms: float | None = Field(default=None, ge=0) + command_age_ms: float | None = Field(default=None, ge=0) + pose_xy: tuple[float, float] + pose_yaw: float + speed_mps: float + applied_speed_mps: float = Field(ge=-1, le=1) + applied_yaw_rate_rps: float = Field(ge=-1, le=1) + stop_reason: Literal[ + "none", "paused", "stale-camera", "stale-command", "inference-error", "unstable" + ] + decision: Decision | None = None + ai_ready: bool + stream_ready: bool + camera: Literal["follow", "overview", "camera"] diff --git a/src/k1link/simulation/ai_polygon/inference.py b/src/k1link/simulation/ai_polygon/inference.py new file mode 100644 index 0000000..57b46f2 --- /dev/null +++ b/src/k1link/simulation/ai_polygon/inference.py @@ -0,0 +1,158 @@ +"""Adapters for the pinned reference DDRNet and RF-DETR model contracts. + +The simulation RGB pinhole profile is separate from the device's KB4 profile. +No device valid-FOV mask or recorded LiDAR calibration is applied to simulator RGB. +""" + +import csv +import http.client +from pathlib import Path +from urllib.parse import urlsplit + +import numpy as np +from PIL import Image + +from k1link.perception.rf_detr_object_detector import ( + COCO_SPARSE_TO_CONTIGUOUS, + RISK_CLASS_IDS, + TritonRfDetrHttpInferenceBackend, + preprocess_raw_kb4_rf_detr, +) + +MODEL_IDS = ["ddrnet-goose-pytorch-reference", "rf_detr_large"] +GOOSE_SURFACE_IDS = (3, 5, 7, 9, 11, 18, 21, 23, 24, 31, 40, 50, 62) + + +def local_endpoint(endpoint: str): + parsed = urlsplit(endpoint) + if ( + parsed.scheme != "http" + or parsed.hostname not in {"127.0.0.1", "localhost", "::1"} + or parsed.path not in {"", "/"} + or parsed.query + or parsed.fragment + or parsed.username + ): + raise ValueError("Use a local Triton endpoint or a loopback tunnel") + return parsed + + +class PillowResizer: + def resize(self, image, width, height): + return np.asarray(Image.fromarray(image).resize((width, height), Image.Resampling.BILINEAR)) + + +class ModelInference: + def __init__( + self, + endpoint: str, + goose_labels: Path, + ddrnet_endpoint: str, + *, + segmenter_id: str = "simulation-ddrnet-goose", + ): + if segmenter_id not in {"simulation-ddrnet-goose", "simulation-segformer-ade"}: + raise ValueError("Uninstalled surface provider") + self.segmenter_id = segmenter_id + parsed = local_endpoint(ddrnet_endpoint) + local_endpoint(endpoint) + with goose_labels.open(newline="") as stream: + labels = {int(row["label_key"]): row["class_name"] for row in csv.DictReader(stream)} + if set(labels) != set(range(64)): + raise ValueError("Expected the existing GOOSE fine-64 label table") + self.road_ids = [ + i + for i, name in labels.items() + if name in {"asphalt", "bikeway", "cobble", "sidewalk", "gravel", "soil"} + ] + if len(self.road_ids) != 6: + raise ValueError("GOOSE road label mapping is incomplete") + self.connection = http.client.HTTPConnection( + parsed.hostname, parsed.port or 8000, timeout=10 + ) + self.detector = TritonRfDetrHttpInferenceBackend(endpoint, timeout_seconds=10) + self.detector_url = local_endpoint(endpoint) + + def ready(self): + self.connection.request("GET", "/ready") + response = self.connection.getresponse() + response.read(65536) + if response.status != 200: + raise RuntimeError("Selected surface provider is unavailable") + connection = http.client.HTTPConnection( + self.detector_url.hostname, self.detector_url.port or 8000, timeout=10 + ) + try: + connection.request("GET", "/v2/models/rf_detr_large/versions/1/ready") + response = connection.getresponse() + response.read(65536) + if response.status != 200: + raise RuntimeError("RF-DETR is unavailable") + finally: + connection.close() + + def segment(self, rgb: np.ndarray): + return self.surface(rgb)["segmentation.labels"] + + def surface(self, rgb: np.ndarray): + if rgb.shape != (600, 800, 3) or rgb.dtype != np.uint8: + raise ValueError("Expected an 800x600 RGB simulation camera") + self.connection.request( + "POST", + "/infer", + body=np.ascontiguousarray(rgb).tobytes(), + headers={"Content-Type": "application/octet-stream"}, + ) + response = self.connection.getresponse() + ade = self.segmenter_id == "simulation-segformer-ade" + expected = 512**2 * (2 if ade else 1) + raw = response.read(expected + 1) + if response.status != 200 or len(raw) != expected: + raise RuntimeError("Surface provider response changed") + mask = np.frombuffer(raw[: 512**2], dtype=np.uint8).reshape(512, 512) + if np.any(mask >= (150 if ade else 64)): + raise RuntimeError("Surface label taxonomy changed") + if ade: + candidate = np.frombuffer(raw[512**2 :], dtype=np.uint8).reshape(512, 512) + if np.any(candidate > 1): + raise RuntimeError("Surface candidate raster changed") + candidate = candidate.astype(bool) + else: + candidate = np.isin(mask, GOOSE_SURFACE_IDS) + return {"segmentation.labels": mask.copy(), "segmentation.surface": candidate} + + def detect(self, rgb: np.ndarray): + if rgb.shape != (600, 800, 3) or rgb.dtype != np.uint8: + raise ValueError("Expected an 800x600 RGB simulation camera") + bgr = np.ascontiguousarray(rgb[:, :, ::-1]) + detector_tensor = preprocess_raw_kb4_rf_detr( + bgr, np.ones((600, 800), dtype=bool), resizer=PillowResizer() + ) + output = self.detector.infer(detector_tensor) + if ( + output.boxes.shape != (1, 300, 4) + or output.logits.shape != (1, 300, 91) + or not np.isfinite(output.boxes).all() + or not np.isfinite(output.logits).all() + ): + raise RuntimeError("RF-DETR output contract changed") + probabilities = 1 / (1 + np.exp(-np.clip(output.logits[0].astype(np.float32), -80, 80))) + classes = [ + key for key, value in COCO_SPARSE_TO_CONTIGUOUS.items() if value in RISK_CLASS_IDS + ] + scores = probabilities[:, classes].max(axis=1) + boxes = [] + for x, y, width, height in output.boxes[0][scores >= 0.25]: + if width <= 0 or height <= 0: + continue + # Unlike the recorded diagnostic filter, never discard a very large near obstacle. + box = np.clip([x - width / 2, y - height / 2, x + width / 2, y + height / 2], 0, 1) + boxes.append(tuple(float(value) for value in box)) + return boxes + + def infer(self, rgb: np.ndarray): + return np.isin(self.segment(rgb), self.road_ids), self.detect(rgb) + + def close(self): + self.connection.close() + self.detector.close() diff --git a/src/k1link/simulation/ai_polygon/mission_policy.py b/src/k1link/simulation/ai_polygon/mission_policy.py new file mode 100644 index 0000000..9d77a21 --- /dev/null +++ b/src/k1link/simulation/ai_polygon/mission_policy.py @@ -0,0 +1,116 @@ +"""Bounded waypoint mission and progress recovery, driven only by observations. + +Operator waypoints specify the task. They are not a collision map or a motion +trajectory. CMU retains authority to reject every requested local waypoint. +""" + +import math + +import numpy as np + + +def inclination(pose): + x, y = pose[3:5] + return math.degrees(math.acos(max(-1, min(1, 1 - 2 * (x * x + y * y))))) + + +class WaypointMission: + def __init__(self, route): + self.route = [list(p) for p in route] + self.index = self.attempts = 0 + self.anchor = self.anchor_time = None + self.last_time = None + self.state = "following" + self.failed_goals = [] + self.goal = None + self.fault = None + self.best_distance = None + self.recovery_goal = None + self.recovery_started = None + + def resume(self): + # Pause freezes the world clock. Preserve the mission cursor and any + # latched failure. The paused world has not moved: retain the observed + # goal entering the camera blind strip, but revalidate it before motion. + self.anchor = self.anchor_time = None + + def update(self, pose, seconds, choose_goal, choose_recovery=None): + xy = np.asarray(pose[:2]) + if self.last_time is not None and seconds < self.last_time: + raise ValueError("Mission observations must not rewind") + self.last_time = seconds + if inclination(pose) >= 30: + self.fault = "unstable" + if self.fault: + return None, self.intent(self.fault) + if self.route and np.linalg.norm(xy - self.route[self.index]) <= 0.4: + if self.index == len(self.route) - 1: + self.fault = "goal-reached" + return None, self.intent(self.fault) + self.index += 1 + self.anchor = self.goal = None + self.best_distance = None + self.recovery_goal = None + self.attempts = 0 + target = self.route[self.index] if self.route else None + if self.anchor is None: + self.anchor, self.anchor_time = xy.copy(), seconds + distance = float(np.linalg.norm(xy - target)) if target is not None else None + if self.best_distance is None: + self.best_distance = distance + progress = self.best_distance - distance if target is not None else 0.0 + if target is None and self.goal is not None: + direction = np.asarray(self.goal[:2]) - self.anchor + progress = float( + np.dot(xy - self.anchor, direction) / max(np.linalg.norm(direction), 0.1) + ) + if self.recovery_goal is not None: + remaining = np.linalg.norm(xy - self.recovery_goal[:2]) + observed = choose_recovery(self.recovery_goal) if choose_recovery is not None else None + if remaining > 0.3 and seconds - self.recovery_started < 6 and observed is not None: + return self.recovery_goal, self.intent("reversing") + # Recovery is an attempt to escape, never route progress. The next + # forward attempt must beat the previous best distance to the task. + self.recovery_goal = self.goal = None + self.anchor_time = seconds + return None, self.intent("replanning") + if progress >= 0.1: + # A reverse/forward cycle cannot replenish the recovery budget. + self.anchor, self.anchor_time = xy.copy(), seconds + self.best_distance = distance + self.attempts = 0 + self.failed_goals.clear() + stalled = seconds - self.anchor_time >= 8 + if stalled: + self.attempts += 1 + if self.goal is not None: + self.failed_goals.append(self.goal) + self.goal = None + self.anchor_time = seconds + if self.attempts > 3: + self.fault = "stuck" + return None, self.intent("stuck") + self.state = "replanning" + if choose_recovery is not None: + self.recovery_goal = choose_recovery(None) + if self.recovery_goal is not None: + self.recovery_started = seconds + return self.recovery_goal, self.intent("reversing") + observed = choose_goal(target, self.goal, self.failed_goals) + if observed is None: + # A rejected frame stops motion, not causal memory. Forgetting the + # last observed exact waypoint strands it behind the near camera + # boundary on the next frame. choose_goal must revalidate visible + # support, and live range/collision checks retain final authority. + return None, self.intent("no-road") + self.goal = observed + self.state = "replanning" if self.attempts else "following" + return self.goal, self.intent(self.state) + + def intent(self, state): + return dict( + state=state, + waypoint=self.index, + waypoint_count=len(self.route), + recovery_attempt=self.attempts, + ) diff --git a/src/k1link/simulation/ai_polygon/policy.py b/src/k1link/simulation/ai_polygon/policy.py new file mode 100644 index 0000000..90ffc4a --- /dev/null +++ b/src/k1link/simulation/ai_polygon/policy.py @@ -0,0 +1,79 @@ +"""Camera-only laboratory road follower. No scene truth or actor coordinates enter here. + +This deliberately small baseline measures model-driven following/braking. It is +not a route planner, metric obstacle-distance estimator, or field safety policy. +""" + +import numpy as np + +from k1link.simulation.ai_polygon.contracts import Decision + + +class RoadPolicy: + def __init__(self, max_speed_mps: float): + if not 0 < max_speed_mps <= 1: + raise ValueError("invalid laboratory speed") + self.max_speed = max_speed_mps + self.clear_frames = 0 + + def reset(self): + self.clear_frames = 0 + + def decide( + self, road_mask: np.ndarray, boxes: list[tuple[float, float, float, float]] + ) -> Decision: + if road_mask.shape != (512, 512) or road_mask.dtype != np.bool_: + raise ValueError("road mask must be the model's 512x512 boolean raster") + # Detector boxes are normalized in the full RGB camera, not the DDRNet crop. + if any( + not all(np.isfinite(box)) + or not (0 <= box[0] <= box[2] <= 1 and 0 <= box[1] <= box[3] <= 1) + for box in boxes + ): + raise ValueError("invalid observation box") + obstacles = sum(x1 < 0.68 and x2 > 0.32 and y2 > 0.55 for x1, _, x2, y2 in boxes) + near_road = road_mask[320:500, 100:412] + fraction = float(near_road.mean()) + stop = "obstacle" if obstacles else "no-road" if fraction < 0.35 else None + if stop: + self.clear_frames = 0 + return Decision( + speed_mps=0, + yaw_rate_rps=0, + reason=stop, + road_fraction=fraction, + obstacle_count=obstacles, + ) + self.clear_frames += 1 + if self.clear_frames < 3: + return Decision( + speed_mps=0, + yaw_rate_rps=0, + reason="uncertain", + road_fraction=fraction, + obstacle_count=0, + ) + # Choose a contiguous visible corridor, rather than averaging two disconnected roads. + columns = near_road.mean(axis=0) >= 0.6 + edges = np.flatnonzero(np.diff(np.r_[False, columns, False].astype(np.int8))) + spans = [(a, b) for a, b in zip(edges[::2], edges[1::2], strict=True) if b - a >= 48] + if not spans: + self.clear_frames = 0 + return Decision( + speed_mps=0, + yaw_rate_rps=0, + reason="uncertain", + road_fraction=fraction, + obstacle_count=0, + ) + left, right = min(spans, key=lambda span: abs((span[0] + span[1]) / 2 - 156)) + center = (left + right) / 2 + yaw = float(np.clip((156 - center) / 156, -0.6, 0.6)) + speed = self.max_speed * min(1, fraction / 0.65) * (1 - abs(yaw)) + return Decision( + speed_mps=float(speed), + yaw_rate_rps=yaw, + reason="road", + road_fraction=fraction, + obstacle_count=0, + ) diff --git a/src/k1link/simulation/ai_polygon/runs.py b/src/k1link/simulation/ai_polygon/runs.py new file mode 100644 index 0000000..4127839 --- /dev/null +++ b/src/k1link/simulation/ai_polygon/runs.py @@ -0,0 +1,412 @@ +"""Single-worker laboratory lifecycle and durable observation/decision journal.""" + +import base64 +import hashlib +import io +import json +import os +import re +import secrets +import shutil +import stat +import threading +import time +from pathlib import Path +from uuid import uuid4 + +from PIL import Image + +from k1link.artifacts import utc_now_iso +from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue +from k1link.simulation.ai_polygon.contracts import ( + RealtimeSnapshot, + RunApplied, + RunCreate, + RunSample, + WorkerHello, +) +from k1link.simulation.ai_polygon.worlds import WorldStore, write_json + +RUN_ID = re.compile(r"^airun-[a-f0-9]{32}$") +TERMINAL = {"completed", "stopped", "failed"} +WORKER_LEASE_SECONDS = 20 + + +class RunStore: + def __init__(self, worlds: WorldStore, queue: ObservatoryRecordedJobQueue | None = None): + self.worlds = worlds + self.queue = queue + self.root = worlds.root.parent / "runs" + self.root.mkdir(mode=0o700, exist_ok=True) + self.lock = threading.RLock() + self.worker: dict | None = None + self.seen = 0.0 + self.active: str | None = None + self.token_path = self.root.parent / "worker.token" + if not self.token_path.exists(): + try: + fd = os.open(self.token_path, os.O_CREAT | os.O_EXCL | os.O_WRONLY, 0o600) + with os.fdopen(fd, "w") as stream: + stream.write(secrets.token_urlsafe(48)) + except FileExistsError: + pass + metadata = self.token_path.lstat() + if not stat.S_ISREG(metadata.st_mode) or metadata.st_mode & 0o077: + raise ValueError("AI polygon worker token must be a private regular file") + with os.fdopen(os.open(self.token_path, os.O_RDONLY | os.O_NOFOLLOW), "r") as stream: + self.token = stream.read(513).strip() + if not 32 <= len(self.token) <= 512: + raise ValueError("AI polygon worker token is invalid") + # Keep realtime ownership uncertain until the same Worker reconciles it. + # A Core restart never proves that the remote GPU/process stopped. + for row in self.list(): + if row["state"] not in TERMINAL: + if row.get("clock") == "realtime": + if self.active is not None: + raise RuntimeError("Multiple unreconciled simulation owners") + self.active = row["run_id"] + row.update(state="disconnected", message="Ожидаем состояние Worker.") + else: + row.update(state="failed", message="Связь с симуляцией прервана перезапуском.") + self._save(row) + + def directory(self, run_id: str) -> Path: + if not RUN_ID.fullmatch(run_id): + raise FileNotFoundError(run_id) + path = self.root / run_id + if not path.is_dir() or path.is_symlink(): + raise FileNotFoundError(run_id) + return path + + def get(self, run_id: str) -> dict: + return json.loads((self.directory(run_id) / "run.json").read_text()) + + def list(self) -> list[dict]: + return sorted( + [ + self.get(p.name) + for p in self.root.iterdir() + if RUN_ID.fullmatch(p.name) and p.is_dir() and not p.is_symlink() + ], + key=lambda item: item["created_at"], + reverse=True, + ) + + def _save(self, doc: dict) -> None: + write_json(self.directory(doc["run_id"]) / "run.json", doc) + + def _expire(self) -> None: + if self.worker is not None and time.monotonic() - self.seen > WORKER_LEASE_SECONDS: + if self.active: + row = self.get(self.active) + if row.get("clock") == "realtime": + row.update( + state="disconnected", + message="Связь с Worker потеряна. Состояние уточняется.", + ) + else: + row.update(state="failed", message="Worker потерял связь. Прогон остановлен.") + self.active = None + self._save(row) + self.worker = None + + def status(self) -> dict: + with self.lock: + self._expire() + return { + "available": self.worker is not None, + "worker": self.worker, + "active_run": self.get(self.active) if self.active else None, + } + + def register(self, hello: WorkerHello) -> dict: + with self.lock: + self._expire() + if self.worker and self.worker["instance_id"] != hello.instance_id: + raise RuntimeError("Другой Worker уже подключён.") + if self.active and self.get(self.active)["worker"] != hello.model_dump(): + raise RuntimeError("Нельзя менять модели во время прогона.") + self.worker = hello.model_dump() + self.seen = time.monotonic() + return {"registered": True} + + def heartbeat(self, instance_id: str) -> dict: + with self.lock: + self._require_worker(instance_id) + self.seen = time.monotonic() + return {"alive": True} + + def _require_worker(self, instance_id: str) -> None: + self._expire() + if self.worker is None or self.worker["instance_id"] != instance_id: + raise RuntimeError("Сессия Worker истекла; требуется переподключение.") + + def start(self, request: RunCreate, request_id: str) -> dict: + if not re.fullmatch(r"[a-zA-Z0-9_-]{8,100}", request_id): + raise ValueError("Требуется уникальный идентификатор запуска.") + with self.lock: + self._expire() + for old in self.list(): + if old["request_id"] == request_id: + if old["request"] != request.model_dump(): + raise RuntimeError("Идентификатор запуска уже использован.") + return old + if self.worker is None: + raise RuntimeError("Подключите Worker с симуляцией и моделями inference.") + if request.clock not in self.worker.get("execution_modes", ["lockstep"]): + raise RuntimeError("Worker не поддерживает этот режим симуляции.") + if request.clock == "realtime" and not self.worker.get("stream"): + raise RuntimeError("Видеопоток Worker не настроен.") + if self.queue is None: + raise RuntimeError("Контроль занятости Worker недоступен.") + if self.active is not None: + raise RuntimeError("Сначала завершите текущий прогон.") + world = self.worlds.get(request.world_id) + storage = world.get("storage", {}) + if storage.get("kind") == "worker" and ( + storage["worker_id"] != self.worker["worker_id"] or request.clock != "realtime" + ): + raise RuntimeError("Локация доступна только на подготовленном Worker.") + if world["status"] != "available" or not world["settings"]["prepared"]: + raise RuntimeError("Сначала проверьте масштаб, грунт и старт ровера.") + run_id = f"airun-{uuid4().hex}" + if ( + request.clock == "lockstep" + and shutil.disk_usage(self.root).free < request.max_steps * 1024**2 + 512 * 1024**2 + ): + raise ValueError("Недостаточно места для кадров прогона.") + (self.root / run_id).mkdir(mode=0o700) + if request.clock == "lockstep": + (self.root / run_id / "frames").mkdir(mode=0o700) + row = { + "schema_version": "missioncore.ai-polygon-run/v1", + "run_id": run_id, + "request_id": request_id, + "request": request.model_dump(), + "created_at": utc_now_iso(), + "world": world, + "worker": dict(self.worker), + "state": "starting", + "control": "pause" if request.start_paused else "play", + "control_sequence": 0, + "camera": "follow", + "telemetry": None, + "step_budget": 0, + "samples": 0, + "applied_steps": 0, + "last_applied": None, + "last_sample": None, + "message": None, + "phase": "world", + "authority": "virtual-only", + "clock": request.clock, + "step_ns": 100_000_000, + } + self._save(row) + try: + self.queue.reserve_simulation(run_id) + except RuntimeError: + row.update(state="failed", message="Worker занят другой задачей.") + self._save(row) + raise + self.active = run_id + return row + + def control(self, run_id: str, command: str) -> dict: + with self.lock: + self._expire() + row = self.get(run_id) + if row["state"] in TERMINAL: + return row + if command not in {"pause", "play", "step", "stop"}: + raise ValueError("Неизвестная команда симуляции.") + if command == "step" and row.get("clock") == "realtime": + raise RuntimeError("Realtime не допускает пошаговое продвижение времени.") + if command == "step" and row["state"] != "paused": + raise RuntimeError("Один шаг доступен только на паузе.") + if row["control"] == "stop": + return row + if row["control"] == command: + return row + row["control_sequence"] = row.get("control_sequence", 0) + 1 + row["control"] = "pause" if command == "step" else command + row["step_budget"] = 1 if command == "step" else 0 + # Paused is acknowledged by the Worker, not inferred from this request. + if command == "stop": + row["state"] = "stopping" + self._save(row) + return row + + def poll(self, instance_id: str, run_id: str | None) -> dict: + with self.lock: + self._require_worker(instance_id) + self.seen = time.monotonic() + if self.active is None: + return {"run": None, "action": "idle"} + row = self.get(self.active) + if run_id is None: + return {"run": row, "action": "load"} + if run_id != self.active: + raise RuntimeError("Прогон Worker не совпадает с активным.") + action = row["control"] + if row.get("clock") == "realtime": + # Only actual local snapshots can acknowledge pause/play. + return {"run": row, "action": action} + if action == "pause": + if row["step_budget"]: + row["step_budget"] = 0 + action = "step" + else: + row["state"] = "paused" + elif action == "play" and row["samples"]: + row["state"] = "running" + self._save(row) + return {"run": row, "action": action} + + def progress(self, run_id: str, instance_id: str, phase: str) -> dict: + with self.lock: + self._require_worker(instance_id) + if run_id != self.active: + raise RuntimeError("Прогон уже завершён.") + row = self.get(run_id) + if row["state"] == "starting": + row["phase"] = phase + self._save(row) + return {"control": row["control"]} + + def view(self, run_id: str, camera: str) -> dict: + with self.lock: + row = self.get(run_id) + if run_id != self.active or row.get("clock") != "realtime": + raise RuntimeError("Симуляция не запущена.") + if camera not in {"follow", "overview", "camera"}: + raise ValueError("Неизвестная камера.") + row["camera"] = camera + self._save(row) + return row + + def snapshot(self, run_id: str, instance_id: str, snapshot: RealtimeSnapshot) -> dict: + with self.lock: + self._require_worker(instance_id) + row = self.get(run_id) + if self.active != run_id or row.get("clock") != "realtime": + raise RuntimeError("Прогон не принадлежит realtime Worker.") + if row["worker"]["instance_id"] != instance_id: + raise RuntimeError("Прогон принадлежит другому Worker.") + old = row.get("telemetry") + if old and snapshot.sequence <= old["sequence"]: + return { + "recorded": old["sequence"] + } # Same snapshot may be retried after reconnect. + if snapshot.control_sequence > row.get("control_sequence", 0): + raise ValueError("Worker подтвердил неизвестную команду.") + if old and snapshot.simulation_time_ns < old["simulation_time_ns"]: + raise ValueError("Время Worker не может идти назад.") + if abs(snapshot.applied_speed_mps) > row["world"]["settings"]["max_speed_mps"]: + raise ValueError("Команда превышает скорость прогона.") + row.update( + telemetry=snapshot.model_dump(), + phase=snapshot.phase, + samples=snapshot.inference_count, + message=None, + ) + if row["control"] != "stop": + row["state"] = snapshot.state + row["telemetry_received_at"] = utc_now_iso() + self._save(row) + self.seen = time.monotonic() + return {"recorded": snapshot.sequence} + + def sample(self, run_id: str, instance_id: str, sample: RunSample) -> dict: + with self.lock: + self._require_worker(instance_id) + if run_id != self.active: + raise RuntimeError("Прогон уже завершён.") + row = self.get(run_id) + if row.get("clock") != "lockstep": + raise RuntimeError("Realtime кадры хранятся только на Worker.") + if row["control"] == "stop": + raise RuntimeError("Получена команда остановки.") + if sample.sequence != row["samples"] or sample.sequence >= row["request"]["max_steps"]: + raise RuntimeError("Нарушена последовательность кадров.") + if row["samples"] != row["applied_steps"]: + raise RuntimeError("Предыдущий шаг физики не подтверждён.") + if sample.simulation_time_ns != sample.sequence * row["step_ns"]: + raise ValueError("Время кадра не соответствует шагу симуляции.") + if abs(sample.decision.speed_mps) > row["world"]["settings"]["max_speed_mps"]: + raise ValueError("Команда превышает скорость прогона.") + try: + image = base64.b64decode(sample.image_jpeg_base64, validate=True) + if not 4 <= len(image) <= 1024**2: + raise ValueError() + with Image.open(io.BytesIO(image)) as decoded: + if decoded.format != "JPEG" or decoded.size != (800, 600): + raise ValueError() + decoded.verify() + except Exception as exc: + raise ValueError("Неверный кадр камеры.") from exc + directory = self.directory(run_id) + if shutil.disk_usage(directory).free < len(image) + 512 * 1024**2: + raise ValueError("Недостаточно места для кадра прогона.") + filename = f"{sample.sequence:06d}.jpg" + (directory / "frames" / filename).write_bytes(image) + recorded = sample.model_dump(exclude={"image_jpeg_base64"}) + recorded.update(image_sha256=hashlib.sha256(image).hexdigest(), image=filename) + with (directory / "decisions.jsonl").open("a", encoding="utf-8") as stream: + stream.write(json.dumps(recorded, allow_nan=False) + "\n") + stream.flush() + os.fsync(stream.fileno()) + row.update(samples=row["samples"] + 1, last_sample=recorded, phase="running") + if row["state"] == "starting": + row["state"] = "running" + self._save(row) + return {"recorded": sample.sequence} + + def applied(self, run_id: str, instance_id: str, receipt: RunApplied) -> dict: + with self.lock: + self._require_worker(instance_id) + if self.active != run_id: + raise RuntimeError("Прогон уже завершён.") + row = self.get(run_id) + if row.get("clock") != "lockstep": + raise RuntimeError("Realtime не использует подтверждения физических шагов.") + if receipt.sequence != row["applied_steps"] or row["samples"] != receipt.sequence + 1: + raise RuntimeError("Шаг не соответствует решению модели.") + if receipt.simulation_time_ns != (receipt.sequence + 1) * row["step_ns"]: + raise ValueError("Неверное время завершения шага.") + recorded = receipt.model_dump() + with (self.directory(run_id) / "motion.jsonl").open("a", encoding="utf-8") as stream: + stream.write(json.dumps(recorded, allow_nan=False) + "\n") + stream.flush() + os.fsync(stream.fileno()) + row.update(applied_steps=row["applied_steps"] + 1, last_applied=recorded) + self._save(row) + return {"applied": receipt.sequence} + + def finish(self, run_id: str, instance_id: str, outcome: str, message: str) -> dict: + with self.lock: + self._require_worker(instance_id) + row = self.get(run_id) + if row["worker"]["instance_id"] != instance_id: + raise RuntimeError("Прогон принадлежит другому Worker.") + if row["state"] in TERMINAL: + if self.queue is not None: + self.queue.release_simulation(run_id) + return row + if run_id != self.active: + raise RuntimeError("Прогон уже завершён.") + if ( + row.get("clock") == "lockstep" + and outcome == "completed" + and ( + row["samples"] != row["request"]["max_steps"] + or row["applied_steps"] != row["samples"] + ) + ): + raise RuntimeError("Прогон не достиг заданного числа шагов.") + row.update(state=outcome, message=message or None) + self._save(row) + if self.queue is not None: + self.queue.release_simulation(run_id) + self.active = None + return row diff --git a/src/k1link/simulation/ai_polygon/terrain_contract.py b/src/k1link/simulation/ai_polygon/terrain_contract.py new file mode 100644 index 0000000..7748caa --- /dev/null +++ b/src/k1link/simulation/ai_polygon/terrain_contract.py @@ -0,0 +1,24 @@ +"""Collision identity is independent of episode start, heading and camera.""" + + +def terrain_matches(manifest, world, generator_sha256=None): + if manifest.get("source_sha256") != world["sha256"]: + return False + if generator_sha256 and manifest.get("generator_sha256") != generator_sha256: + return False + old, new = manifest["settings"], world["settings"] + if any(old[key] != new[key] for key in ("meters_per_unit", "rotation_degrees")): + return False + if manifest.get("generator") == "paired-source": + # A supplied full-scene collider is not the generated 30 m tile below. + # Source/calibration and collider hashes still bind this asset; actual + # support and full-body clearance are checked at the new start by Isaac. + return manifest.get("collider_sha256") == world.get("collider_sha256") and bool( + manifest.get("collider_sha256") + ) + # The versioned preparer captures a 30x30 m tile and a 12 m vertical band. + # Admit starts only within its interior; physical support is checked later. + return ( + all(abs(old["spawn_xy"][i] - new["spawn_xy"][i]) <= 12 for i in (0, 1)) + and abs(old["ground_z"] - new["ground_z"]) <= 2 + ) diff --git a/src/k1link/simulation/ai_polygon/worlds.py b/src/k1link/simulation/ai_polygon/worlds.py new file mode 100644 index 0000000..c56f81e --- /dev/null +++ b/src/k1link/simulation/ai_polygon/worlds.py @@ -0,0 +1,256 @@ +"""Durable, resumable Gaussian asset admission, independent of XGRIDS sources.""" + +import hashlib +import json +import os +import re +import shutil +import threading +from pathlib import Path +from uuid import uuid4 + +import numpy as np + +from k1link.artifacts import utc_now_iso +from k1link.simulation.ai_polygon.contracts import WorkerWorldCreate, WorldCreate, WorldSettings + +CHUNK_BYTES = 4 * 1024**2 +WORLD_ID = re.compile(r"^aiworld-[a-f0-9]{32}$") +SOURCES = [ + { + "name": "Forest Scan", + "author": "draftmode", + "license": "CC BY 4.0", + "source_url": "https://superspl.at/scene/259c0051", + "description": "Лесная тропа и папоротники", + }, + { + "name": "Bamboo Trail", + "author": "luckysplat", + "license": "CC BY 4.0", + "source_url": "https://superspl.at/scene/dd49e9a8", + "description": "Тропа в бамбуковой роще", + }, +] + + +def write_json(path: Path, value: object) -> None: + temporary = path.with_name(f".{path.name}-{uuid4().hex}") + try: + with temporary.open("x", encoding="utf-8") as stream: + os.chmod(temporary, 0o600) + json.dump(value, stream, ensure_ascii=False, allow_nan=False) + stream.flush() + os.fsync(stream.fileno()) + temporary.replace(path) + finally: + temporary.unlink(missing_ok=True) + + +def inspect_gaussian_ply(path: Path) -> int: + """Admit only standard scalar binary 3DGS; do not label meshes as splats.""" + with path.open("rb") as stream: + header = bytearray() + while len(header) < 65536: + line = stream.readline(1024) + header.extend(line) + if line.rstrip() == b"end_header": + break + if not line: + raise ValueError("В PLY отсутствует заголовок Gaussian-сцены.") + else: + raise ValueError("Заголовок PLY превышает допустимый размер.") + try: + lines = bytes(header).decode("ascii").splitlines() + except UnicodeDecodeError as exc: + raise ValueError("Некорректный заголовок PLY.") from exc + if lines[:2] != ["ply", "format binary_little_endian 1.0"]: + raise ValueError("Экспортируйте стандартный Gaussian PLY (binary little-endian).") + elements = [line for line in lines if line.startswith("element ")] + if len(elements) != 1 or not elements[0].startswith("element vertex "): + raise ValueError("Нужен Gaussian PLY с одним элементом vertex, без меша.") + count = int(elements[0].split()[-1]) + properties = [line.split() for line in lines if line.startswith("property ")] + names = [prop[-1] for prop in properties] + required = { + "x", + "y", + "z", + "opacity", + "f_dc_0", + "f_dc_1", + "f_dc_2", + "scale_0", + "scale_1", + "scale_2", + "rot_0", + "rot_1", + "rot_2", + "rot_3", + } + if ( + not 1 <= count <= 20_000_000 + or not required.issubset(names) + or len(names) != len(set(names)) + or not 14 <= len(properties) <= 128 + or any(len(prop) != 3 or prop[1] not in {"float", "float32"} for prop in properties) + ): + raise ValueError("PLY не содержит поддерживаемые Gaussian-атрибуты.") + if path.stat().st_size != len(header) + count * len(properties) * 4: + raise ValueError("Размер PLY не соответствует его заголовку.") + with path.open("rb") as stream: + stream.seek(len(header)) + for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""): + if not np.isfinite(np.frombuffer(chunk, dtype=" Path: + if not WORLD_ID.fullmatch(world_id): + raise FileNotFoundError(world_id) + path = self.root / world_id + if path.is_symlink() or not path.is_dir(): + raise FileNotFoundError(world_id) + return path + + def get(self, world_id: str) -> dict: + with self.lock: + path = self.directory(world_id) + document = json.loads((path / "world.json").read_text()) + if document["status"] == "uploading": + source = path / "source.part" + if not source.exists(): + source = path / "source.ply" + document["uploaded_bytes"] = source.stat().st_size + return document + + def list(self) -> list[dict]: + return sorted( + [ + self.get(p.name) + for p in self.root.iterdir() + if WORLD_ID.fullmatch(p.name) and p.is_dir() and not p.is_symlink() + ], + key=lambda item: item["created_at"], + reverse=True, + ) + + def create(self, request: WorldCreate) -> dict: + with self.lock: + if shutil.disk_usage(self.root).free < request.byte_length + 512 * 1024**2: + raise ValueError("Недостаточно места для исходника сцены.") + world_id = f"aiworld-{uuid4().hex}" + path = self.root / world_id + path.mkdir(mode=0o700) + (path / "source.part").touch(mode=0o600) + document = { + "schema_version": "missioncore.ai-polygon-world/v1", + "world_id": world_id, + **request.model_dump(mode="json"), + "status": "uploading", + "uploaded_bytes": 0, + "sha256": None, + "splat_count": None, + "created_at": utc_now_iso(), + "settings": WorldSettings().model_dump(mode="json"), + } + write_json(path / "world.json", document) + return document + + def register_worker_asset(self, request: WorkerWorldCreate, worker_id: str) -> dict: + """Keep only an authenticated asset manifest on the operator machine.""" + with self.lock: + payload = request.model_dump(mode="json") + storage = {"kind": "worker", "worker_id": worker_id} + for existing in self.list(): + if existing.get("storage") == storage and existing["sha256"] == request.sha256: + if any(existing.get(k) != v for k, v in payload.items() if k != "settings"): + raise RuntimeError("Манифест сохранённой локации изменился.") + return existing + world_id = f"aiworld-{uuid4().hex}" + path = self.root / world_id + path.mkdir(mode=0o700) + document = { + "schema_version": "missioncore.ai-polygon-world/v1", + "world_id": world_id, + **payload, + "storage": storage, + "status": "available", + "uploaded_bytes": 0, + "created_at": utc_now_iso(), + } + write_json(path / "world.json", document) + return document + + def append(self, world_id: str, offset: int, payload: bytes) -> dict: + with self.lock: + doc = self.get(world_id) + if doc["status"] != "uploading" or offset != doc["uploaded_bytes"]: + raise RuntimeError("Позиция загрузки изменилась. Возобновите передачу.") + if not 0 < len(payload) <= CHUNK_BYTES or offset + len(payload) > doc["byte_length"]: + raise ValueError("Размер фрагмента загрузки недопустим.") + if shutil.disk_usage(self.root).free < len(payload) + 512 * 1024**2: + raise ValueError("Недостаточно свободного места.") + part = self.directory(world_id) / "source.part" + with part.open("r+b") as stream: + stream.seek(offset) + stream.write(payload) + stream.flush() + os.fsync(stream.fileno()) + return self.get(world_id) + + def complete(self, world_id: str) -> dict: + with self.lock: + doc = self.get(world_id) + if doc["status"] == "available": + return doc + if doc["uploaded_bytes"] != doc["byte_length"]: + raise RuntimeError("Загрузка сцены ещё не завершена.") + directory = self.directory(world_id) + source = directory / "source.part" + # Recover an interrupted atomic publication without discarding the source. + if not source.exists(): + source = directory / "source.ply" + count = inspect_gaussian_ply(source) + digest = hashlib.sha256() + with source.open("rb") as stream: + for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""): + digest.update(chunk) + source.replace(directory / "source.ply") + doc.update(status="available", sha256=digest.hexdigest(), splat_count=count) + write_json(directory / "world.json", doc) + return doc + + def prefix_hashes(self, world_id: str) -> dict: + """Verify a resumed file against every byte already admitted, in bounded chunks.""" + with self.lock: + doc = self.get(world_id) + if doc.get("storage", {}).get("kind") == "worker": + raise RuntimeError("Исходник локации хранится на Worker.") + directory = self.directory(world_id) + source = directory / "source.part" + if not source.exists(): + source = directory / "source.ply" + chunks = [] + with source.open("rb") as stream: + for chunk in iter(lambda: stream.read(CHUNK_BYTES), b""): + chunks.append( + {"byte_length": len(chunk), "sha256": hashlib.sha256(chunk).hexdigest()} + ) + return {"uploaded_bytes": doc["uploaded_bytes"], "chunks": chunks} + + def configure(self, world_id: str, settings: WorldSettings) -> dict: + with self.lock: + doc = self.get(world_id) + if doc["status"] != "available": + raise RuntimeError("Сначала завершите импорт сцены.") + doc["settings"] = settings.model_dump(mode="json") + write_json(self.directory(world_id) / "world.json", doc) + return doc diff --git a/src/k1link/web/ai_polygon_api.py b/src/k1link/web/ai_polygon_api.py new file mode 100644 index 0000000..b8581aa --- /dev/null +++ b/src/k1link/web/ai_polygon_api.py @@ -0,0 +1,231 @@ +"""Control Station and authenticated simulation-worker ports for AI polygon.""" + +import secrets +from pathlib import Path +from typing import Literal + +from fastapi import APIRouter, Header, HTTPException, Request, Response +from fastapi.responses import FileResponse + +from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue +from k1link.simulation.ai_polygon.composition import compose, registry +from k1link.simulation.ai_polygon.contracts import ( + RealtimeSnapshot, + RunApplied, + RunCreate, + RunProgress, + RunSample, + ViewControl, + WorkerHello, + WorkerPoll, + WorkerResult, + WorkerWorldCreate, + WorldCreate, + WorldSettings, +) +from k1link.simulation.ai_polygon.runs import RunStore +from k1link.simulation.ai_polygon.worlds import CHUNK_BYTES, SOURCES, WorldStore + + +def build_ai_polygon_router( + data_dir: Path, queue: ObservatoryRecordedJobQueue | None = None +) -> APIRouter: + worlds = WorldStore(data_dir) + runs = RunStore(worlds, queue) + router = APIRouter(prefix="/api/v1/ai-polygon", tags=["ai-polygon"]) + adapters = Path(__file__).resolve().parents[3] / "simulation/ai-polygon" + + def invoke(fn, *args): + try: + return fn(*args) + except FileNotFoundError as exc: + raise HTTPException(404, "Локация или прогон не найдены.") from exc + except ValueError as exc: + raise HTTPException(400, str(exc)) from exc + except RuntimeError as exc: + raise HTTPException(409, str(exc)) from exc + + def authenticate(authorization: str | None) -> None: + expected = f"Bearer {runs.token}" + if not authorization or not secrets.compare_digest(authorization, expected): + raise HTTPException(401, "Worker authentication required") + + @router.get("/catalog") + def catalog(): + return { + "schema_version": "missioncore.ai-polygon-catalog/v1", + "sources": SOURCES, + "worlds": worlds.list(), + "runtime": runs.status(), + "runs": runs.list()[:30], + } + + @router.post("/worlds", status_code=201) + def create_world(body: WorldCreate): + return invoke(worlds.create, body) + + @router.get("/worlds/{world_id}") + def get_world(world_id: str): + return invoke(worlds.get, world_id) + + @router.get("/worlds/{world_id}/upload-prefix") + def upload_prefix(world_id: str): + return invoke(worlds.prefix_hashes, world_id) + + @router.patch("/worlds/{world_id}/source") + async def upload( + world_id: str, request: Request, upload_offset: int = Header(alias="Upload-Offset", ge=0) + ): + data = bytearray() + async for block in request.stream(): + if len(data) + len(block) > CHUNK_BYTES: + raise HTTPException(413, "Фрагмент загрузки превышает 4 МБ.") + data.extend(block) + return invoke(worlds.append, world_id, upload_offset, bytes(data)) + + @router.post("/worlds/{world_id}/complete") + def complete(world_id: str): + return invoke(worlds.complete, world_id) + + @router.put("/worlds/{world_id}/settings") + def settings(world_id: str, body: WorldSettings): + return invoke(worlds.configure, world_id, body) + + @router.get("/worlds/{world_id}/source.ply") + def source(world_id: str): + row = invoke(worlds.get, world_id) + if row["status"] != "available": + raise HTTPException(409, "Импорт не завершён.") + if row.get("storage", {}).get("kind") == "worker": + raise HTTPException(409, "Исходник локации хранится на Worker.") + return FileResponse( + worlds.directory(world_id) / "source.ply", + media_type="application/octet-stream", + headers={ + "ETag": f'"{row["sha256"]}"', + "Cache-Control": "private, max-age=31536000, immutable", + }, + ) + + @router.post("/runs", status_code=201) + def start(body: RunCreate, idempotency_key: str = Header(alias="Idempotency-Key")): + if body.clock == "realtime": + graph = invoke(compose, adapters, body.composition) + body = body.model_copy(update={"composition": graph.selection_document()}) + return invoke(runs.start, body, idempotency_key) + + @router.get("/ai-modules") + def modules(): + installed = invoke(registry, adapters) + graph = invoke(compose, adapters) + return { + "catalog": {**installed.catalog(), "authority": "virtual-only"}, + "selection": graph.selection_document(), + "composition_sha256": graph.sha256, + } + + @router.get("/runs/{run_id}") + def get_run(run_id: str): + runs.status() + return invoke(runs.get, run_id) + + @router.post("/runs/{run_id}/{command}") + def control(run_id: str, command: Literal["pause", "play", "step", "stop"]): + return invoke(runs.control, run_id, command) + + @router.get("/runs/{run_id}/frames/{sequence}.jpg") + def frame(run_id: str, sequence: int): + row = invoke(runs.get, run_id) + if not 0 <= sequence < row["samples"]: + raise HTTPException(404, "Кадр не найден.") + return FileResponse( + runs.directory(run_id) / "frames" / f"{sequence:06d}.jpg", + media_type="image/jpeg", + headers={"Cache-Control": "private, max-age=31536000, immutable"}, + ) + + @router.put("/runs/{run_id}/view") + def view(run_id: str, body: ViewControl): + return invoke(runs.view, run_id, body.camera) + + @router.get("/runs/{run_id}/decisions") + def decisions(run_id: str): + directory = invoke(runs.directory, run_id) + path = directory / "decisions.jsonl" + if not path.exists(): + return Response("", media_type="application/x-ndjson") + return FileResponse(path, media_type="application/x-ndjson", filename="decisions.jsonl") + + @router.post("/worker/register") + def register(body: WorkerHello, authorization: str | None = Header(default=None)): + authenticate(authorization) + return invoke(runs.register, body) + + @router.post("/worker/worlds", status_code=201) + def register_worker_world( + body: WorkerWorldCreate, + authorization: str | None = Header(default=None), + instance_id: str = Header(alias="Worker-Instance"), + ): + authenticate(authorization) + with runs.lock: + invoke(runs.heartbeat, instance_id) + worker = runs.status()["worker"] + return invoke(worlds.register_worker_asset, body, worker["worker_id"]) + + @router.post("/worker/heartbeat") + def heartbeat(body: WorkerPoll, authorization: str | None = Header(default=None)): + authenticate(authorization) + return invoke(runs.heartbeat, body.instance_id) + + @router.post("/worker/poll") + def poll(body: WorkerPoll, authorization: str | None = Header(default=None)): + authenticate(authorization) + return invoke(runs.poll, body.instance_id, body.run_id) + + @router.post("/worker/runs/{run_id}/progress") + def progress( + run_id: str, + body: RunProgress, + instance_id: str = Header(alias="Worker-Instance"), + authorization: str | None = Header(default=None), + ): + authenticate(authorization) + return invoke(runs.progress, run_id, instance_id, body.phase) + + @router.post("/worker/runs/{run_id}/samples") + def sample( + run_id: str, + body: RunSample, + instance_id: str = Header(alias="Worker-Instance"), + authorization: str | None = Header(default=None), + ): + authenticate(authorization) + return invoke(runs.sample, run_id, instance_id, body) + + @router.post("/worker/runs/{run_id}/snapshot") + def snapshot( + run_id: str, + body: RealtimeSnapshot, + instance_id: str = Header(alias="Worker-Instance"), + authorization: str | None = Header(default=None), + ): + authenticate(authorization) + return invoke(runs.snapshot, run_id, instance_id, body) + + @router.post("/worker/runs/{run_id}/finish") + def finish(run_id: str, body: WorkerResult, authorization: str | None = Header(default=None)): + authenticate(authorization) + return invoke(runs.finish, run_id, body.instance_id, body.outcome, body.message) + + @router.post("/worker/runs/{run_id}/applied") + def applied( + run_id: str, + body: RunApplied, + instance_id: str = Header(alias="Worker-Instance"), + authorization: str | None = Header(default=None), + ): + authenticate(authorization) + return invoke(runs.applied, run_id, instance_id, body) + + return router diff --git a/src/k1link/web/app.py b/src/k1link/web/app.py index 0fbc3ed..b4eb7d3 100644 --- a/src/k1link/web/app.py +++ b/src/k1link/web/app.py @@ -238,6 +238,7 @@ from k1link.missions.registration_runs import RegistrationRuns from k1link.web.mission_registration_api import build_mission_registration_router from k1link.web.mission_planner_api import build_mission_planner_router from k1link.web.simulation_projects_api import build_simulation_projects_router +from k1link.web.ai_polygon_api import build_ai_polygon_router from k1link.web.simulation_world_provider_api import build_simulation_world_provider_router from k1link.web.system_telemetry_api import build_system_telemetry_router from k1link.web.vegetation_shadow_lab_api import ( @@ -1996,6 +1997,10 @@ app.include_router( ) ) frontend_dist = REPOSITORY_ROOT / "apps" / "control-station" / "dist" +try: + app.include_router(build_ai_polygon_router(session_store.data_dir, OBSERVATORY_RECORDED_JOB_QUEUE)) +except (OSError, ValueError): + logging.getLogger(__name__).exception("AI polygon could not load its private runtime state") app.include_router( build_viewer_diagnostics_router( expected_ui_build_id=lambda: frontend_build_id(frontend_dist), diff --git a/tests/test_ai_polygon.py b/tests/test_ai_polygon.py new file mode 100644 index 0000000..57731c6 --- /dev/null +++ b/tests/test_ai_polygon.py @@ -0,0 +1,398 @@ +"""Synthetic protocol checks; these do not qualify rendering or AI model quality.""" + +import base64 +import hashlib +import io +import json + +import numpy as np +import pytest +from fastapi import FastAPI +from fastapi.testclient import TestClient +from PIL import Image +from pydantic import ValidationError +from test_observatory_recorded_jobs import _definitions + +from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue +from k1link.simulation.ai_polygon.contracts import ( + Decision, + RunApplied, + RunCreate, + RunSample, + WorkerHello, + WorkerWorldCreate, + WorldCreate, + WorldSettings, +) +from k1link.simulation.ai_polygon.policy import RoadPolicy +from k1link.simulation.ai_polygon.runs import RunStore +from k1link.simulation.ai_polygon.worlds import WorldStore, inspect_gaussian_ply +from k1link.web.ai_polygon_api import build_ai_polygon_router + + +def ply(value=0.0): + names = [ + "x", + "y", + "z", + "opacity", + "f_dc_0", + "f_dc_1", + "f_dc_2", + "scale_0", + "scale_1", + "scale_2", + "rot_0", + "rot_1", + "rot_2", + "rot_3", + ] + header = "ply\nformat binary_little_endian 1.0\nelement vertex 1\n" + header += "".join(f"property float {name}\n" for name in names) + "end_header\n" + return header.encode() + np.full(14, value, dtype=" 0 and 0 < decision.speed_mps < 0.3 + + +def test_progress_does_not_claim_camera_ready(runs): + store, hello, world = runs + run = store.start(RunCreate(world_id=world["world_id"]), "progress-case-001") + store.progress(run["run_id"], hello.instance_id, "scene") + assert store.get(run["run_id"])["phase"] == "scene" + assert store.get(run["run_id"])["state"] == "starting" + store.control(run["run_id"], "stop") + assert store.progress(run["run_id"], hello.instance_id, "models")["control"] == "stop" + assert store.get(run["run_id"])["state"] == "stopping" + + +def test_worker_stop_race_accepts_already_exited_process(monkeypatch): + import importlib.util + from pathlib import Path + + runtime = Path(__file__).resolve().parents[1] / "simulation/ai-polygon" + monkeypatch.syspath_prepend(str(runtime)) + spec = importlib.util.spec_from_file_location("polygon_worker_stop_test", runtime / "worker.py") + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + calls = [] + + class Child: + pid = 123 + + def poll(self): + return None + + def wait(self, timeout): + calls.append(("wait", timeout)) + return 0 + + def taskkill(*args, **kwargs): + # Windows reports a non-zero taskkill when the process exits in the race. + assert kwargs["check"] is False + calls.append(("taskkill", 255)) + + monkeypatch.setattr(module.subprocess, "run", taskkill) + module.terminate_episode(Child()) + assert calls == [("taskkill", 255), ("wait", 30)] diff --git a/tests/test_ai_polygon_mission.py b/tests/test_ai_polygon_mission.py new file mode 100644 index 0000000..880fbcf --- /dev/null +++ b/tests/test_ai_polygon_mission.py @@ -0,0 +1,141 @@ +import numpy as np +import pytest + +from k1link.simulation.ai_polygon.contracts import WorldSettings +from k1link.simulation.ai_polygon.mission_policy import WaypointMission, inclination + + +def pose(x=0, y=0): + return [x, y, 0.37, 0, 0, 0, 1] + + +def goal(target, prior, excluded): + return [2, 0.7 * len(excluded), 0] + + +def test_chassis_jitter_cannot_hide_stall_and_retries_are_bounded(): + mission = WaypointMission([[8, 0]]) + states = [] + for t in np.arange(0, 34, 0.2): + _, intent = mission.update(pose(0.01 * np.sin(t * 10)), float(t), goal) + states.append(intent["state"]) + assert "replanning" in states + assert states[-1] == "stuck" + assert mission.update(pose(1), 35, goal)[1]["state"] == "stuck" + + +def test_route_cursor_and_completion_survive_pause_without_restarting_task(): + mission = WaypointMission([[1, 0], [3, 0]]) + mission.update(pose(), 0, goal) + assert mission.update(pose(1), 6, goal)[1]["waypoint"] == 1 + mission.resume() + assert mission.update(pose(1), 6, goal)[1]["waypoint"] == 1 + assert mission.update(pose(3), 18, goal)[1]["state"] == "goal-reached" + mission.resume() + assert mission.update(pose(3), 18, goal)[1]["state"] == "goal-reached" + + +def test_overturned_and_excessive_tilt_are_latched_before_goal_success(): + mission = WaypointMission([[0, 0]]) + overturned = [0, 0, 0.37, 1, 0, 0, 0] + assert inclination(overturned) == pytest.approx(180) + assert mission.update(overturned, 0, goal)[1]["state"] == "unstable" + mission.resume() + assert mission.update(pose(), 1, goal)[1]["state"] == "unstable" + + +def test_missing_surface_never_becomes_a_drive_permission_or_unbounded_wait(): + mission = WaypointMission([[8, 0]]) + for t in range(34): + target, intent = mission.update(pose(), t, lambda *_: None) + assert target is None + assert intent["state"] == "stuck" + + +def test_route_contract_bounds_and_finiteness(): + from pydantic import ValidationError + + for route in ([[float("nan"), 0]], [[0, 0]] * 33, [[0, 10001]]): + with pytest.raises(ValidationError): + WorldSettings(route_xy=route) + + +def test_recovery_uses_observed_goal_and_does_not_count_retreat_as_progress(): + mission = WaypointMission([[8, 0]]) + + def retreat(prior): + return prior or [-0.65, 0, 0] + + mission.update(pose(), 0, goal, retreat) + target, intent = mission.update(pose(), 8, goal, retreat) + assert target == [-0.65, 0, 0] and intent["state"] == "reversing" + assert mission.update(pose(-0.2), 10, goal, retreat)[1]["state"] == "reversing" + assert mission.update(pose(-0.4), 12, goal, retreat)[1]["state"] == "replanning" + mission.update(pose(), 16, goal, retreat) + assert mission.attempts == 1 + # Moving back to where we started cannot create a fresh three-attempt budget. + assert mission.update(pose(), 20, goal, retreat)[1]["recovery_attempt"] == 2 + + +def test_recovery_stops_immediately_on_lost_support_and_times_out_without_motion(): + mission = WaypointMission([[8, 0]]) + + def retreat(prior): + return prior or [-0.65, 0, 0] + + mission.update(pose(), 0, goal, retreat) + mission.update(pose(), 8, goal, retreat) + assert mission.update(pose(), 8.2, goal, lambda _: None)[0] is None + assert mission.recovery_goal is None + for seconds in (17, 23, 32, 38, 47): + _, intent = mission.update(pose(), seconds, goal, retreat) + assert intent["state"] == "stuck" + + +def test_wrong_direction_never_replenishes_route_attempts(): + mission = WaypointMission([[8, 0]]) + for seconds in range(34): + _, intent = mission.update(pose(-seconds * 0.1), seconds, goal) + assert intent["state"] == "stuck" + + +def test_signed_motion_contract_accepts_reverse_but_stays_bounded(): + from pydantic import ValidationError + + from k1link.simulation.ai_polygon.contracts import Decision + + assert ( + Decision( + speed_mps=-0.1, yaw_rate_rps=0, reason="replanning", road_fraction=0, obstacle_count=0 + ).speed_mps + < 0 + ) + with pytest.raises(ValidationError): + Decision(speed_mps=-1.1, yaw_rate_rps=0, reason="road", road_fraction=1, obstacle_count=0) + + +def test_slow_regulated_progress_is_not_mistaken_for_stall(): + mission = WaypointMission([[8, 0]]) + for seconds in range(60): + _, intent = mission.update(pose(seconds * 0.02), seconds, goal) + assert intent["state"] == "following" + assert intent["recovery_attempt"] == 0 + + +def test_rejected_observation_stops_but_does_not_forget_revalidated_goal(): + mission = WaypointMission([[2, 0]]) + observed = [2, 0, 0] + assert mission.update(pose(), 0, lambda *_: observed)[0] == observed + assert mission.update(pose(1), 1, lambda *_: None)[0] is None + assert mission.goal == observed + mission.resume() + calls = [] + + def revalidate(_target, prior, _excluded): + calls.append(prior) + return prior + + assert mission.update(pose(1.3), 1.2, revalidate)[0] == observed + assert calls == [observed] + # Memory alone never authorizes a command when the new frame is rejected. + assert mission.update(pose(1.3), 1.4, lambda *_: None)[0] is None diff --git a/tests/test_ai_polygon_motion.py b/tests/test_ai_polygon_motion.py new file mode 100644 index 0000000..868dd45 --- /dev/null +++ b/tests/test_ai_polygon_motion.py @@ -0,0 +1,40 @@ +"""The actuator may soften acceleration but must never prolong a safety command.""" + +import importlib.util +from pathlib import Path + +import pytest + +spec = importlib.util.spec_from_file_location( + "motion_control", Path(__file__).parents[1] / "simulation/ai-polygon/motion_control.py" +) +module = importlib.util.module_from_spec(spec) +spec.loader.exec_module(module) + + +def test_slow_perception_keeps_continuous_physics_command(): + drive = module.DriveEnvelope() + rows = [drive.step(0.15, 0, 1 / 60)[0] for _ in range(180)] + assert rows[0] == pytest.approx(0.2 / 60) + assert all(0 <= b - a <= 0.2 / 60 + 1e-9 for a, b in zip(rows, rows[1:], strict=False)) + assert rows[44:] == pytest.approx([0.15] * 136) + + +def test_braking_and_deadman_bypass_ramp(): + drive = module.DriveEnvelope() + for _ in range(60): + drive.step(0.15, 0, 1 / 60) + assert drive.step(0.03, 0, 1 / 60) == (0.03, 0) + assert drive.step(0.15, 0.2, 1 / 60, stop=True) == (0, 0) + assert drive.wheels == (0, 0) + assert drive.step(0.15, 0, 1 / 60)[0] == pytest.approx(0.2 / 60) + + +def test_curvature_and_direction_change(): + drive = module.DriveEnvelope() + for _ in range(60): + speed, yaw = drive.step(0.15, 0.2, 1 / 60) + assert yaw / speed == pytest.approx(0.2 / 0.15) + speed, yaw = drive.step(-0.15, -0.2, 1 / 60) + assert speed < 0 and yaw < 0 + assert max(map(abs, drive.wheels)) <= 0.2 / 60 + 1e-9 diff --git a/tests/test_ai_polygon_navigation.py b/tests/test_ai_polygon_navigation.py new file mode 100644 index 0000000..3ab5140 --- /dev/null +++ b/tests/test_ai_polygon_navigation.py @@ -0,0 +1,390 @@ +import importlib.util +import subprocess +import sys +from pathlib import Path + +import numpy as np +import pytest + +from k1link.observatory.modular_composition import CompositionError +from k1link.simulation.ai_polygon.composition import compose, registry +from k1link.simulation.ai_polygon.terrain_contract import terrain_matches + +ROOT = Path(__file__).resolve().parents[1] / "simulation/ai-polygon" + + +def module(name): + spec = importlib.util.spec_from_file_location(name, ROOT / (name + ".py")) + result = importlib.util.module_from_spec(spec) + spec.loader.exec_module(result) + return result + + +def test_simulation_composition_has_causal_dependencies_and_separate_authority(): + graph = compose(ROOT) + assert graph.as_dict()["execution"]["mode"] == "worker-local-simulation" + motion = graph.nodes[-1] + assert motion.module.group == "motion" + assert dict(motion.inputs)["segmentation.surface"] == "simulation-segformer-ade" + assert dict(motion.inputs)["detection.boxes"] == "simulation-rf-detr" + assert motion.module.state_policy == "causal-reset-at-source-start" + selection = graph.selection_document() + selection["selections"] = [r for r in selection["selections"] if r["group"] != "segmentation"] + with pytest.raises(CompositionError, match="segmentation.surface"): + compose(ROOT, selection) + + +def test_surface_providers_are_interchangeable_in_the_shared_constructor(): + selection = compose(ROOT).selection_document() + reference = next(m for m in registry(ROOT).modules if m.module_id == "simulation-ddrnet-goose") + for row in selection["selections"]: + if row["group"] == "segmentation": + row.update(module_id=reference.module_id, module_sha256=reference.sha256) + graph = compose(ROOT, selection) + assert dict(graph.nodes[-1].inputs)["segmentation.surface"] == reference.module_id + + +def test_composition_rejects_stale_module_identity(): + selection = compose(ROOT).selection_document() + selection["selections"][0]["module_sha256"] = "0" * 64 + with pytest.raises(CompositionError, match="not installed"): + compose(ROOT, selection) + + +def test_shared_constructor_import_needs_no_core_or_third_party_runtime(): + # -S removes site-packages, as in the minimal Windows coordinator. Loading + # a contract must not load the POSIX-only artifact gateway through __init__. + source = str(ROOT.parents[1] / "src") + result = subprocess.run( + [ + sys.executable, + "-S", + "-c", + f"import sys; sys.path.insert(0, {source!r}); " + "from k1link.observatory.modular_composition import ModuleRegistry; " + "from k1link.simulation.ai_polygon.composition import compose; " + "assert 'k1link.artifact_gateway' not in sys.modules", + ], + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr + + +def test_semantic_goal_uses_range_and_correct_square_camera_crop(): + nav = module("navigation_client") + points = np.array( + [[x, y, 0] for x in np.linspace(1.5, 3, 20) for y in np.linspace(-0.3, 0.3, 9)], + dtype=np.float32, + ) + pose = [0, 0, 0.27, 0, 0, 0, 1] + calibration = { + "origin": [0.38, 0, 0.8], + "rotation": np.eye(3).reshape(-1).tolist(), + "intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300], + } + leaves = np.ones((512, 512), dtype=bool) + goal = nav.visual_goal(leaves, points, pose, calibration) + assert goal is not None and 1.5 < goal[0] < 3 and abs(goal[1]) < 0.3 + assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration) is None + assert nav.visual_goal(leaves, points + [0, 0, 2], pose, calibration) is None + # A previously valid goal cannot authorize motion through newly unknown RGB. + assert nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, goal) is None + # An explicit-route waypoint entering the camera blind strip is retained, + # but losing all current visual surface support still forbids movement. + close = [0.65, 0, 0] + assert nav.visual_goal(leaves, points, pose, calibration, close, target=[0.65, 0]) == close + assert ( + nav.visual_goal(np.zeros_like(leaves), points, pose, calibration, close, target=[0.65, 0]) + is None + ) + + +def test_ground_placement_uses_actual_triangle_intersection(): + terrain = module("terrain") + vertices = np.array([[0, 0, 0], [1, 0, 0.2], [0, 1, 0]], dtype=np.float32) + faces = np.array([[0, 1, 2]], dtype=np.int32) + assert terrain.ground_intersections(vertices, faces, 0.25, 0.25)[0] == pytest.approx(0.05) + assert len(terrain.ground_intersections(vertices, faces, 0.9, 0.9)) == 0 + + +def test_observed_route_goal_keeps_task_position_and_cannot_run_away_from_it(): + choose = module("navigation_client").visual_goal + points = np.array( + [[x, y, 0] for x in np.arange(1.2, 3.1, 0.05) for y in np.arange(-0.5, 0.51, 0.05)] + ) + pose = [0, 0, 0.37, 0, 0, 0, 1] + calibration = { + "origin": [0.38, 0, 0.8], + "rotation": np.eye(3).reshape(-1).tolist(), + "intrinsics": [800 * 24 / 36, 800 * 24 / 36, 400, 300], + "body_contact_height_m": 0.37, + } + surface = np.ones((512, 512), dtype=bool) + target = [2.03, 0.27] + assert choose(surface, points, pose, calibration, target=target) == pytest.approx([*target, 0]) + # The close, already observed waypoint can enter the camera blind strip. + advanced = [1.5, 0, 0.37, 0, 0, 0, 1] + camera = {**calibration, "origin": [1.88, 0, 0.8]} + prior = [*target, 0] + assert choose(surface, points + [1.5, 0, 0], advanced, camera, prior, target) == prior + # The actual camera loses nearby ground beyond the old hardcoded 0.8 m. + advanced = [1.1, 0, 0.37, 0, 0, 0, 1] + camera = {**calibration, "origin": [1.48, 0, 0.8]} + assert choose(surface, points + [1.1, 0, 0], advanced, camera, prior, target) == prior + assert choose(np.zeros_like(surface), points, pose, calibration, target=target) is None + # Clear road ahead is not permission to drive away from a missed waypoint. + assert choose(surface, points, pose, calibration, target=[-1, 0]) is None + # A distant task may still use an observed local goal towards it. + far = choose(surface, points, pose, calibration, target=[8, 0]) + assert far is not None and 1.2 <= far[0] <= 3.1 + + +def test_collision_identity_follows_geometry_and_tile_coverage_not_camera_or_start(): + settings = dict( + meters_per_unit=1, + rotation_degrees=[-90, 0, 180], + spawn_xy=[0, 0], + ground_z=0, + camera_height_m=0.8, + max_speed_mps=0.15, + ) + terrain = dict(source_sha256="a" * 64, generator_sha256="b" * 64, settings=settings) + world = dict(sha256="a" * 64, settings={**settings, "spawn_xy": [1, 1], "camera_height_m": 1}) + assert terrain_matches(terrain, world, "b" * 64) + assert not terrain_matches(terrain, world, "c" * 64) + assert not terrain_matches(terrain, {**world, "sha256": "d" * 64}) + for change in ({"spawn_xy": [20, 0]}, {"meters_per_unit": 2}, {"rotation_degrees": [0, 0, 0]}): + assert not terrain_matches(terrain, {**world, "settings": {**world["settings"], **change}}) + + +def test_paired_full_scene_does_not_inherit_generated_tile_bounds(): + settings = dict(meters_per_unit=1, rotation_degrees=[90, 0, 0], spawn_xy=[0, 0], ground_z=5) + terrain = dict( + generator="paired-source", + source_sha256="a" * 64, + collider_sha256="b" * 64, + settings=settings, + ) + world = dict( + sha256="a" * 64, + collider_sha256="b" * 64, + settings={**settings, "spawn_xy": [210, 30], "ground_z": 1.5}, + ) + assert terrain_matches(terrain, world) + assert not terrain_matches(terrain, {**world, "collider_sha256": "c" * 64}) + assert not terrain_matches( + terrain, {**world, "settings": {**world["settings"], "meters_per_unit": 2}} + ) + assert not terrain_matches({**terrain, "generator": "generated-tile"}, world) + + +def test_square_footprint_fits_straight_corridor_and_rejects_corner_sweep(): + check = module("navigation/footprint").swept_footprint_clear + pose = [0, 0, 0.27, 0, 0, 0, 1] + path = [[0, 0, 0], [0.5, 0, 0], [1, 0, 0]] + walls = np.array([[x, y, 0.5, 0.5] for x in np.arange(-1, 2, 0.1) for y in [-0.65, 0.65]]) + assert check(path, walls, pose) + walls[:, 1] *= 0.45 / 0.65 + assert not check(path, walls, pose) + # A diagonal turn sweeps a square corner into this obstacle, even though + # the chassis at its initial and final straight poses does not contain it. + assert not check([[0, 0, 0], [0.5, 0.5, 0]], [[0.7, 0, 0.5, 0.5]], pose) + + +def test_command_monitor_covers_deadman_braking_distance_and_rotation(): + check = module("navigation/footprint").command_footprint_clear + pose = [0, 0, 0.27, 0, 0, 0, 1] + assert check(0.15, 0, [[1.5, 0, 0.5, 0.5]], pose) + assert not check(0.15, 0, [[0.7, 0, 0.5, 0.5]], pose) + assert not check(0, 0.8, [[0.7, 0, 0.5, 0.5]], pose) + + +def test_smooth_slope_is_distinct_from_a_step_or_vertical_terrain(): + costs = module("navigation/terrain_costs").supported_slope_costs + xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)]) + slope = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15))) + normalized, corrected = costs(slope) + assert corrected > len(slope) * 0.9 + assert normalized[len(slope) // 2, 3] == 0 + assert np.array_equal(normalized[:, :3], slope[:, :3]) + # A 15 cm ledge across the initial footprint cannot become a traversable ramp. + step = slope.copy() + step[:, 2] = np.where(step[:, 0] >= 0, 0.15, 0) + assert costs(step)[1] == 0 + cliff = slope.copy() + cliff[:, 2] = np.where(cliff[:, 0] >= 0, -0.4, 0) + assert costs(cliff)[1] == 0 + steep = slope.copy() + steep[:, 2] = steep[:, 0] * np.tan(np.radians(35)) + assert costs(steep)[1] == 0 + assert costs(slope[np.abs(slope[:, 1]) < 0.01])[1] == 0 # Unobserved lateral support. + + +def test_underbody_support_does_not_clear_future_terrain_walls_or_drops(): + correct = module("navigation/terrain_costs").underbody_support_costs + terrain = np.array( + [ + [-0.375, -0.28, 0.066, 0.103], # Low return already under the chassis. + [0.46, 0, 0.066, 0.103], # Inset excludes the leading edge. + [0.75, 0, 0.066, 0.103], # Never change future terrain from body pose. + [0, 0, 0.12, 0.12], # A real step within the footprint remains blocked. + [0, 0, 0.5, 0.5], + [0, 0, -0.4, 0.4], + ] + ) + original = terrain.copy() + result, count = correct(terrain, [0, 0, 0.37, 0, 0, 0, 1]) + assert count == 1 and result[0, 3] == pytest.approx(0.066) + assert np.array_equal(result[1:], original[1:]) + assert np.array_equal(terrain, original) # Never erase the causal raw map. + # Rotate both observations and the measured chassis; the result must agree. + yaw = np.pi / 2 + rotated = terrain.copy() + rotated[:, :2] = terrain[:, :2] @ np.array([[0, 1], [-1, 0]]) + [3, 4] + pose = [3, 4, 0.37, 0, 0, np.sin(yaw / 2), np.cos(yaw / 2)] + assert np.allclose(correct(rotated, pose)[0][:, 3], result[:, 3]) + assert correct(terrain, [0, 0, 0.37, 0, np.sin(np.pi / 12), 0, np.cos(np.pi / 12)])[1] == 0 + + +def test_retreat_requires_observed_full_width_support_and_no_drop_or_step(): + choose = module("navigation_client").recovery_goal + points = np.array( + [[x, y, 0.0] for x in np.arange(-1.5, -0.39, 0.05) for y in np.arange(-0.85, 0.86, 0.05)] + ) + pose = [0, 0, 0.37, 0, 0, 0, 1] + calibration = {"body_contact_height_m": 0.37} + assert choose(points, pose, calibration) == pytest.approx([-0.65, 0, 0]) + assert choose(points[points[:, 1] > -0.2], pose, calibration) is None + assert choose(points[points[:, 0] < -0.9], pose, calibration) is None + for height in (-0.4, 0.15): + discontinuous = points.copy() + discontinuous[points[:, 0] < -0.9, 2] = height + assert choose(discontinuous, pose, calibration) is None + slope = points.copy() + slope[:, 2] = slope[:, 0] * np.tan(np.radians(10)) + assert choose(slope, pose, calibration) is not None + assert choose(points, pose, calibration, [-0.65, 0.3, 0]) is None + + +def test_reverse_monitor_checks_behind_the_body(): + check = module("navigation/footprint").command_footprint_clear + pose = [0, 0, 0.37, 0, 0, 0, 1] + assert check(-0.1, 0, [[-1.5, 0, 0.5, 0.5]], pose) + assert not check(-0.1, 0, [[-0.65, 0, 0.5, 0.5]], pose) + + +def test_legacy_coordinate_migration_is_an_exact_rigid_rotation(): + import json + import struct + + migrate = module("navigation/migrate_terrain_coordinates") + positions = [[1.0, -2.0, -3.0], [2.0, -2.0, -3.0], [1.0, -1.0, -3.0]] + binary = struct.pack("<9f3I", *(v for p in positions for v in p), 0, 1, 2) + document = { + "nodes": [{"mesh": 0}], + "meshes": [{"primitives": [{"attributes": {"POSITION": 0}, "indices": 1}]}], + "accessors": [ + { + "bufferView": 0, + "componentType": 5126, + "type": "VEC3", + "count": 3, + "min": [1, -2, -3], + "max": [2, -1, -3], + }, + {"bufferView": 1, "componentType": 5125, "type": "SCALAR", "count": 3}, + ], + "bufferViews": [{"byteOffset": 0, "byteLength": 36}, {"byteOffset": 36, "byteLength": 12}], + } + raw = json.dumps(document).encode() + raw += b" " * ((-len(raw)) % 4) + glb = ( + struct.pack(" len(surface) * 0.8 + for discontinuity in (0.12, 0.15, -0.4): + ledge = surface.copy() + ledge[:, 2] = np.where(xy[:, 0] >= 0, discontinuity, 0) + corrected, _ = costs(ledge) + near_edge = np.abs(xy[:, 0]) < 0.12 + assert np.all(corrected[near_edge, 3] > 0.1) + stone = surface.copy() + stone[:, 2] = 0 + stone[(abs(xy[:, 0]) < 0.12) & (abs(xy[:, 1]) < 0.12), 2] = 0.15 + assert costs(stone)[1] == 0 + + +def test_grade_fit_cannot_bridge_an_unobserved_gap(): + costs = module("navigation/terrain_costs").supported_slope_costs + points = np.array( + [ + [x, y, 0 if x < 0 else 0.15, 0.15] + for x in [-0.3, -0.2, 0.2, 0.3] + for y in np.arange(-0.3, 0.31, 0.1) + ] + ) + assert costs(points)[1] == 0 + + +def test_existing_reserve_overlap_only_allows_departure_not_approach_or_body_overlap(): + check = module("navigation/footprint").command_footprint_clear + pose = [0, 0, 0.37, 0, 0, 0, 1] + behind = [[-0.53, 0, 0.5, 0.5]] + assert check(0.15, 0, behind, pose) + assert not check(-0.1, 0, behind, pose) + assert not check(0, 0.35, behind, pose) + assert not check(0.15, 0, [[-0.49, 0, 0.5, 0.5]], pose) + assert not check(0.15, 0, [[0.53, 0, 0.5, 0.5]], pose) + # A longer admitted camera age must also enlarge the collision envelope. + assert not check(0.15, 0, [[0.80, 0, 0.5, 0.5]], pose) + + +def test_terrain_fit_cache_invalidates_when_a_new_obstacle_is_observed(): + costs = module("navigation/terrain_costs") + normalize = costs.TerrainCostNormalizer() + xy = np.array([[x, y] for x in np.arange(-1, 1.01, 0.1) for y in np.arange(-1, 1.01, 0.1)]) + grade = np.column_stack((xy, xy[:, 0] * np.tan(np.radians(20)), np.full(len(xy), 0.15))) + clear, count = normalize(grade) + assert count > len(grade) * 0.9 + assert np.array_equal(normalize(grade)[0], clear) + changed = np.vstack((grade, [0.02, 0.02, 0.3, 0.3])) + cached, count = normalize(changed) + fresh, expected_count = costs.supported_slope_costs(changed) + assert np.array_equal(cached, fresh) and count == expected_count + assert cached[len(grade) // 2, 3] > 0.1 + assert np.array_equal(normalize(grade)[0], clear) + + +def test_velocity_regulation_keeps_the_selected_arc_and_its_braking_clearance(): + monitor = module("navigation/footprint") + pose = [0, 0, 0.37, 0, 0, 0, 1] + hazard = [[0.834, -0.08, 0.15, 0.15]] + speed, yaw, scale = monitor.regulate_command(0.15, -0.245, hazard, pose) + assert 0.2 <= scale < 1 + assert speed / yaw == pytest.approx(0.15 / -0.245) + assert monitor.command_footprint_clear(speed, yaw, hazard, pose) + assert monitor.regulate_command(0.15, 0, [[0.56, 0, 0.2, 0.2]], pose) == (0, 0, 0) + assert monitor.regulate_command(0.15, 0, [[0.49, 0, 0.2, 0.2]], pose) == (0, 0, 0) + reverse, _, scale = monitor.regulate_command(-0.1, 0, [[-0.68, 0, 0.2, 0.2]], pose) + assert 0.2 <= scale < 1 and reverse < 0 + assert monitor.command_footprint_clear(reverse, 0, [[-0.68, 0, 0.2, 0.2]], pose) diff --git a/tests/test_ai_polygon_realtime.py b/tests/test_ai_polygon_realtime.py new file mode 100644 index 0000000..0d7ee02 --- /dev/null +++ b/tests/test_ai_polygon_realtime.py @@ -0,0 +1,303 @@ +"""Lifecycle and clock-isolation checks; GPU/stream performance is qualified on Worker.""" + +import importlib.util +import threading +import time +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +from pydantic import ValidationError +from test_ai_polygon import make_world, sample +from test_observatory_recorded_jobs import _definitions + +from k1link.observatory.recorded_jobs import ObservatoryRecordedJobQueue +from k1link.simulation.ai_polygon.contracts import ( + RealtimeSnapshot, + RunCreate, + StreamEndpoint, + WorkerHello, +) +from k1link.simulation.ai_polygon.runs import RunStore +from k1link.simulation.ai_polygon.worlds import WorldStore + + +@pytest.fixture +def realtime(tmp_path): + queue = ObservatoryRecordedJobQueue(tmp_path, definitions=_definitions()) + store = RunStore(WorldStore(tmp_path), queue) + hello = WorkerHello( + worker_id="worker-006", + instance_id="a" * 32, + runtime="isaac-sim-6.1", + model_ids=["reference"], + profile_sha256="b" * 64, + runtime_sources={key: "c" * 64 for key in ("worker", "scene", "models", "robot")}, + execution_modes=["realtime"], + stream=StreamEndpoint(server="100.80.1.2"), + ) + store.register(hello) + world = make_world(store.worlds) + row = store.start( + RunCreate(world_id=world["world_id"], clock="realtime", start_paused=True), + "test-realtime-01", + ) + return store, hello, row + + +def snapshot(**changes): + fields = dict( + sequence=0, + control_sequence=0, + state="ready", + phase="running", + simulation_time_ns=0, + wall_elapsed_seconds=1, + physics_steps=0, + render_frames=30, + sensor_frames=0, + inference_count=0, + dropped_frames=0, + rtf=0, + render_fps=30, + sensor_fps=0, + ai_hz=0, + pose_xy=(0, 0), + pose_yaw=0, + speed_mps=0, + applied_speed_mps=0, + applied_yaw_rate_rps=0, + stop_reason="paused", + ai_ready=False, + stream_ready=True, + camera="follow", + ) + return RealtimeSnapshot(**{**fields, **changes}) + + +def test_worker_owns_acknowledgement_and_no_frame_archive_on_core(realtime): + store, hello, row = realtime + key = row["run_id"] + assert not (store.directory(key) / "frames").exists() + assert store.poll(hello.instance_id, key)["run"]["state"] == "starting" + store.snapshot(key, hello.instance_id, snapshot()) + command = store.control(key, "play") + assert command["state"] == "ready" + assert store.control(key, "play")["control_sequence"] == command["control_sequence"] + assert store.poll(hello.instance_id, key)["run"]["state"] == "ready" + store.snapshot( + key, + hello.instance_id, + snapshot(sequence=1, control_sequence=1, state="running", simulation_time_ns=66666667), + ) + assert store.get(key)["state"] == "running" + with pytest.raises(RuntimeError): + store.sample(key, hello.instance_id, sample()) + with pytest.raises(RuntimeError): + store.control(key, "step") + + +def test_disconnect_restart_reconcile_never_releases_gpu_or_replays(realtime): + store, hello, row = realtime + key = row["run_id"] + store.snapshot(key, hello.instance_id, snapshot()) + store.seen -= 21 + assert store.status()["active_run"]["state"] == "disconnected" + with pytest.raises(RuntimeError): + store.queue.reserve_simulation("airun-" + "d" * 32) + recovered = RunStore(store.worlds, store.queue) + with pytest.raises(RuntimeError): + recovered.register(hello.model_copy(update={"instance_id": "d" * 32})) + recovered.register(hello) + assert recovered.poll(hello.instance_id, key)["action"] == "pause" + recovered.snapshot(key, hello.instance_id, snapshot(sequence=3)) + assert recovered.get(key)["state"] == "ready" + recovered.control(key, "stop") + recovered.snapshot(key, hello.instance_id, snapshot(sequence=4)) + assert recovered.get(key)["state"] == "stopping" + recovered.finish(key, hello.instance_id, "stopped", "") + recovered.queue.reserve_simulation("airun-" + "d" * 32) + + +def test_snapshot_is_idempotent_and_rejects_unissued_ack(realtime): + store, hello, row = realtime + key = row["run_id"] + first = snapshot(sequence=5, simulation_time_ns=1000) + assert store.snapshot(key, hello.instance_id, first) == store.snapshot( + key, hello.instance_id, first + ) + with pytest.raises(ValueError): + store.snapshot(key, hello.instance_id, snapshot(sequence=6, control_sequence=2)) + with pytest.raises(ValueError): + store.snapshot(key, hello.instance_id, snapshot(sequence=6, simulation_time_ns=999)) + with pytest.raises(ValidationError): + StreamEndpoint(server="8.8.8.8") + + +def test_slow_ai_is_bounded_and_pause_fences_inflight_result(tmp_path): + path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py" + spec = importlib.util.spec_from_file_location("polygon_realtime_ai", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + entered, release = threading.Event(), threading.Event() + seen = [] + + class Model: + def ready(self): + pass + + def close(self): + pass + + def infer(self, rgb): + seen.append(int(rgb[0, 0, 0])) + entered.set() + assert release.wait(2) + return None, [] + + decision = { + "speed_mps": 0.3, + "yaw_rate_rps": 0, + "reason": "road", + "road_fraction": 0.7, + "obstacle_count": 0, + } + ai = module.LatestInference( + Model, + SimpleNamespace( + reset=lambda: None, decide=lambda *_: SimpleNamespace(model_dump=lambda: decision) + ), + tmp_path, + ) + ai.start() + try: + ai.enable(True) + ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), 0, time.monotonic(), 0) + assert entered.wait(2) + for frame_id in range(1, 11): + ai.submit( + np.full((2, 2, 3), frame_id, dtype=np.uint8), + frame_id, + time.monotonic(), + frame_id * 1000, + ) + assert ai.dropped == 9 + assert ai.command(time.monotonic())[0] == 0 + ai.enable(False) + release.set() + ai.close() + assert seen == [0] + assert ai.result is None + finally: + release.set() + ai.close() + + +def test_expired_source_stops_even_if_inference_just_completed(tmp_path): + path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py" + spec = importlib.util.spec_from_file_location("polygon_command_deadline", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + ai = module.LatestInference(None, None, tmp_path) + ai.enabled = True + ai.result = { + "captured_at": 0.0, + "completed_at": 1.0, + "decision": {"speed_mps": 0.3, "yaw_rate_rps": 0}, + } + assert ai.command(1.1)[:3] == (0.0, 0.0, "stale-camera") + ai.result["captured_at"] = 1.0 + assert ai.command(1.1)[:3] == (0.3, 0, "none") + ai.result["decision"]["speed_mps"] = -0.1 + assert ai.command(1.1)[:3] == (-0.1, 0, "none") + assert ai.command(1.6)[:2] == (0, 0) + # The worker's 0.8 s camera budget still rejects old images, even when + # a result has just arrived, and never extends the command watchdog. + ai.result.update(captured_at=1.0, completed_at=1.7) + assert ai.command(1.79, frame_deadline=0.8)[:3] == (-0.1, 0, "none") + assert ai.command(1.81, frame_deadline=0.8)[:3] == (0.0, 0.0, "stale-camera") + ai.result.update(captured_at=1.7, completed_at=1.0) + assert ai.command(1.79, frame_deadline=0.8)[:2] == (0.0, 0.0) + + +def test_reverse_telemetry_keeps_the_run_speed_limit(realtime): + store, hello, row = realtime + store.snapshot(row["run_id"], hello.instance_id, snapshot(applied_speed_mps=-0.1)) + assert store.get(row["run_id"])["telemetry"]["applied_speed_mps"] == -0.1 + with pytest.raises(ValueError, match="скорость"): + store.snapshot( + row["run_id"], hello.instance_id, snapshot(sequence=1, applied_speed_mps=-0.4) + ) + + +def test_resume_requires_fresh_clear_observations(tmp_path): + from k1link.simulation.ai_polygon.policy import RoadPolicy + + path = Path(__file__).parents[1] / "simulation/ai-polygon/realtime_ai.py" + spec = importlib.util.spec_from_file_location("polygon_resume", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + + class Model: + def ready(self): + pass + + def close(self): + pass + + def infer(self, rgb): + return np.ones((512, 512), dtype=bool), [] + + ai = module.LatestInference(Model, RoadPolicy(0.3), tmp_path) + ai.start() + + def frame(number): + ai.submit(np.zeros((2, 2, 3), dtype=np.uint8), number, time.monotonic(), number) + deadline = time.monotonic() + 2 + while ai.count < number and time.monotonic() < deadline: + time.sleep(0.001) + assert ai.count == number + return ai.command(time.monotonic())[0] + + try: + ai.enable(True) + assert frame(1) == frame(2) == 0 + assert frame(3) > 0 + ai.enable(False) + ai.enable(True) + assert frame(4) == frame(5) == 0 + assert frame(6) > 0 + finally: + ai.close() + + +def test_windows_snapshot_sharing_retry_is_bounded(tmp_path, monkeypatch): + path = Path(__file__).parents[1] / "simulation/ai-polygon/local_state.py" + spec = importlib.util.spec_from_file_location("polygon_ipc", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + original = module.os.replace + attempts = [] + + def transient(source, target): + attempts.append(1) + if len(attempts) <= 2: + raise PermissionError("Windows sharing violation") + original(source, target) + + monkeypatch.setattr(module.os, "replace", transient) + monkeypatch.setattr(module.time, "sleep", lambda _: None) + target = tmp_path / "snapshot.json" + module.write_json(target, {"sequence": 3}) + assert module.read_json(target) == {"sequence": 3} + assert len(attempts) == 3 + attempts.clear() + + def permanent(): + attempts.append(1) + raise PermissionError("Not a transient lock") + + with pytest.raises(PermissionError): + module.sharing_retry(permanent) + assert len(attempts) == 8 diff --git a/tests/test_ai_polygon_spawn.py b/tests/test_ai_polygon_spawn.py new file mode 100644 index 0000000..79e93ef --- /dev/null +++ b/tests/test_ai_polygon_spawn.py @@ -0,0 +1,42 @@ +"""Prevent a reconstructed trunk/face from being admitted between ground rays.""" + +import importlib.util +from pathlib import Path + +import numpy as np + +SPEC = importlib.util.spec_from_file_location( + "spawn_clearance", Path(__file__).parents[1] / "simulation/ai-polygon/spawn_clearance.py" +) +MODULE = importlib.util.module_from_spec(SPEC) +SPEC.loader.exec_module(MODULE) + + +def count(points, xy=(0, 0), heading=0, plane=(0, 0, 0)): + return MODULE.obstructing_triangles(np.array(points), np.array([[0, 1, 2]]), xy, heading, plane) + + +def test_narrow_trunk_between_support_rays_and_crossing_triangle(): + assert count([[0.21, 0.21, 0], [0.23, 0.21, 0], [0.21, 0.21, 2]]) == 1 + # All vertices outside the prism, but the face crosses through its centre. + assert count([[-2, 0, 0.3], [2, 0, 0.3], [0, 2, 0.3]]) == 1 + + +def test_ground_step_and_overhead_geometry_do_not_block_start(): + for z in (0, 0.05, 0.10, 1.5): + assert count([[-2, -2, z], [2, -2, z], [0, 2, z]]) == 0 + assert count([[-2, -2, 0.12], [2, -2, 0.12], [0, 2, 0.12]]) == 1 + + +def test_triangle_bounding_box_alone_does_not_reject_empty_corner(): + assert count([[0.4, 2, 0.5], [2, 0.4, 0.5], [2, 2, 0.5]]) == 0 + + +def test_clearance_tracks_translation_heading_and_support_slope(): + points = np.array([[0.6, 0, 0.3], [0.7, 0, 0.3], [0.6, 0.03, 0.8]]) + assert count(points) == 0 + assert count(points, heading=45) == 1 + points[:, 2] += points[:, 0] * 0.2 + 4 + points[:, :2] += [10, -3] + assert count(points, (10, -3), 45, (0.2, 0, 4)) == 1 + assert count([[8, -5, 3.6], [12, -5, 4.4], [10, -1, 4]], (10, -3), 45, (0.2, 0, 4)) == 0 diff --git a/tests/test_ai_polygon_worker_assets.py b/tests/test_ai_polygon_worker_assets.py new file mode 100644 index 0000000..ed6dfb7 --- /dev/null +++ b/tests/test_ai_polygon_worker_assets.py @@ -0,0 +1,74 @@ +"""Paired assets remain on Worker; hashes and manifests cross the operator link.""" + +import hashlib +import importlib +import json +import struct +from pathlib import Path + +import numpy as np +import pytest +from test_ai_polygon import ply, worker_asset + + +def triangle_glb(): + binary = np.array([[0, 0, 0], [1, 0, 0], [0, 0, 1]], dtype=" None: with pytest.raises(ObservatoryRecordedQueueIntegrityError, match="identity"): queue.get(job.job_id) + + +def test_simulation_reservation_serializes_recorded_claims_and_survives_restart(tmp_path): + queue = _queue(tmp_path) + job, _ = queue.submit(_intent(), enqueue=True) + owner = "airun-" + "a" * 32 + queue.reserve_simulation(owner) + queue.reserve_simulation(owner) + restarted = _queue(tmp_path) + assert restarted.claim_next(claimant_id="worker-006", claim_request_id="sim-blocked") is None + assert restarted.get(job.job_id).state == "queued" + with pytest.raises(ObservatoryRecordedQueueConflictError): + restarted.release_simulation("airun-" + "b" * 32) + restarted.release_simulation(owner) + claim = restarted.claim_next(claimant_id="worker-006", claim_request_id="sim-released") + assert claim.job.job_id == job.job_id + with pytest.raises(ObservatoryRecordedQueueBusyError): + queue.reserve_simulation(owner) + + +def test_simulation_and_recorded_claim_cannot_win_together(tmp_path): + queue = _queue(tmp_path) + queue.submit(_intent(), enqueue=True) + second = _queue(tmp_path) + barrier = Barrier(2) + + def simulate(): + barrier.wait() + try: + queue.reserve_simulation("airun-" + "a" * 32) + return True + except ObservatoryRecordedQueueBusyError: + return False + + def recorded(): + barrier.wait() + return ( + second.claim_next(claimant_id="worker-006", claim_request_id="racing-claim") is not None + ) + + with ThreadPoolExecutor(max_workers=2) as pool: + simulation = pool.submit(simulate) + inference = pool.submit(recorded) + assert simulation.result() != inference.result()