"""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()