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