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