434 lines
18 KiB
Python
434 lines
18 KiB
Python
"""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()
|