450 lines
19 KiB
Python
450 lines
19 KiB
Python
"""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()
|