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