实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Explicit ROS1 navigation / HTTP / ROS2 adapter; defaults are simulation only."""
|
||||
@@ -0,0 +1,230 @@
|
||||
"""Backend boundary. Only backend telemetry can establish readiness or stop evidence."""
|
||||
from abc import ABC, abstractmethod
|
||||
import copy
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
|
||||
|
||||
TERMINAL_CONTROLLERS = frozenset({"SUCCEEDED", "ARRIVED", "ABORTED", "PREEMPTED", "RECALLED", "REJECTED"})
|
||||
|
||||
|
||||
class NavigationBackend(ABC):
|
||||
@abstractmethod
|
||||
def source_is_fresh(self, sample):
|
||||
"""Re-evaluate original source time now; never trust receipt age alone."""
|
||||
|
||||
@abstractmethod
|
||||
def health(self):
|
||||
"""Return ready, reason, map_id, received_at (local monotonic), source_fresh."""
|
||||
|
||||
@abstractmethod
|
||||
def send(self, goal):
|
||||
"""Exactly one attempt; return ACCEPTED, REJECTED, or UNKNOWN. Never retry."""
|
||||
|
||||
@abstractmethod
|
||||
def cancel(self, goal_id):
|
||||
"""Send cancel intent; return value is never stop evidence."""
|
||||
|
||||
@abstractmethod
|
||||
def snapshot(self, goal_id):
|
||||
"""Return correlated controller_state and odom/pose telemetry, or UNKNOWN."""
|
||||
|
||||
|
||||
class MockBackend(NavigationBackend):
|
||||
"""Simulation only: explicit synthetic telemetry, no ROS imports or motion output."""
|
||||
def __init__(self, clock=time.monotonic, map_id="sim-map", auto_complete_sec=None, source_clock=None):
|
||||
self.clock, self.map_id = clock, map_id
|
||||
origin_source, origin_monotonic = time.time(), clock()
|
||||
self.source_clock = source_clock or (lambda: origin_source + clock() - origin_monotonic)
|
||||
self.auto_complete_sec = auto_complete_sec
|
||||
self.send_count = self.cancel_count = self.sequence = 0
|
||||
self.send_mode = "ACCEPTED"
|
||||
self.goals, self.samples, self.cancelled, self.odom_queues = {}, {}, set(), {}
|
||||
self.health_sample(True)
|
||||
|
||||
def source_is_fresh(self, sample):
|
||||
stamp = sample.get("source_stamp")
|
||||
return (sample.get("source_fresh") is True and isinstance(stamp, (int, float))
|
||||
and math.isfinite(stamp) and 0 < stamp <= self.source_clock())
|
||||
|
||||
def health_sample(self, ready):
|
||||
self.health_value = {"ready": ready, "reason": "SIMULATION", "map_id": self.map_id,
|
||||
"received_at": self.clock(), "source_fresh": True, "source_stamp": self.source_clock()}
|
||||
|
||||
def health(self):
|
||||
if self.auto_complete_sec is not None: self.health_sample(True)
|
||||
return copy.deepcopy(self.health_value)
|
||||
|
||||
def send(self, goal):
|
||||
self.send_count += 1
|
||||
self.goals[goal["goal_id"]] = (copy.deepcopy(goal), self.clock())
|
||||
self.samples[goal["goal_id"]] = {"controller_state": "REJECTED" if self.send_mode == "REJECTED" else "ACTIVE"}
|
||||
return self.send_mode
|
||||
|
||||
def cancel(self, goal_id):
|
||||
self.cancel_count += 1
|
||||
self.cancelled.add(goal_id)
|
||||
return True # ACK only; tests supply independent telemetry.
|
||||
|
||||
def set_snapshot(self, goal_id, state, linear=0.0, angular=0.0, pose=None, source_fresh=True):
|
||||
self.sequence += 1
|
||||
self.samples[goal_id] = {"controller_state": state,
|
||||
"odom": {"sequence": self.sequence, "received_at": self.clock(), "source_fresh": source_fresh,
|
||||
"source_stamp": self.source_clock(), "linear": linear, "angular": angular},
|
||||
"pose": {"value": copy.deepcopy(pose), "received_at": self.clock(), "source_fresh": source_fresh,
|
||||
"source_stamp": self.source_clock()}}
|
||||
self.odom_queues.setdefault(goal_id, []).append(copy.deepcopy(self.samples[goal_id]["odom"]))
|
||||
|
||||
def snapshot(self, goal_id):
|
||||
if self.auto_complete_sec is not None and goal_id in self.goals:
|
||||
goal, started = self.goals[goal_id]
|
||||
done = self.clock() - started >= self.auto_complete_sec
|
||||
state = "PREEMPTED" if goal_id in self.cancelled else ("SUCCEEDED" if done else "ACTIVE")
|
||||
self.set_snapshot(goal_id, state, linear=0.0 if state != "ACTIVE" else 0.1, pose=goal["target_pose"])
|
||||
out = copy.deepcopy(self.samples.get(goal_id, {"controller_state": "UNKNOWN"}))
|
||||
out["odom_samples"] = self.odom_queues.pop(goal_id, [])
|
||||
return out
|
||||
|
||||
|
||||
class Ros1MoveBaseBackend(NavigationBackend):
|
||||
"""Optional Noetic adapter. No automatic assumption that move_base is deployed.
|
||||
|
||||
All ROS endpoint names and freshness limits must be configured. A trusted ROS
|
||||
safety/health monitor publishes JSON String: ready, map_id, stamp (ROS seconds),
|
||||
reason; HTTP clients cannot set these values. ROS1 graph must be access controlled.
|
||||
"""
|
||||
def __init__(self, *, action_name, odom_topic, pose_topic, readiness_topic, map_id,
|
||||
source_max_age_sec, server_wait_sec):
|
||||
import rospy
|
||||
import actionlib
|
||||
from actionlib_msgs.msg import GoalStatus
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import String
|
||||
for name in (action_name, odom_topic, pose_topic, readiness_topic, map_id):
|
||||
if not isinstance(name, str) or not name: raise ValueError("ROS endpoints/map_id must be explicit")
|
||||
for value in (source_max_age_sec, server_wait_sec):
|
||||
if not math.isfinite(value) or value <= 0: raise ValueError("ROS timing limits must be explicit and positive")
|
||||
self.rospy, self.MoveBaseGoal = rospy, MoveBaseGoal
|
||||
self.map_id, self.max_age = map_id, source_max_age_sec
|
||||
self.lock, self.sequence = threading.RLock(), 0
|
||||
self.current_id = None
|
||||
self.state = "UNKNOWN"
|
||||
self.odom = self.pose = None
|
||||
self.odom_queue, self.previous_odom_stamp = [], None
|
||||
self._ros_epoch, self._last_ros_time = 0, None
|
||||
self.health_value = {"ready": False, "map_id": map_id, "reason": "health monitor missing", "received_at": 0, "source_fresh": False}
|
||||
self.status_names = {getattr(GoalStatus, name): name for name in
|
||||
("PENDING", "ACTIVE", "PREEMPTED", "SUCCEEDED", "ABORTED", "REJECTED", "PREEMPTING", "RECALLING", "RECALLED", "LOST")}
|
||||
self.client = actionlib.SimpleActionClient(action_name, MoveBaseAction)
|
||||
self.connected = self.client.wait_for_server(rospy.Duration(server_wait_sec))
|
||||
self.subscribers = [rospy.Subscriber(odom_topic, Odometry, self._odom, queue_size=20),
|
||||
rospy.Subscriber(pose_topic, PoseStamped, self._pose, queue_size=10),
|
||||
rospy.Subscriber(readiness_topic, String, self._health, queue_size=1)]
|
||||
|
||||
def _observe_ros_clock(self):
|
||||
"""A backward jump permanently invalidates all previously cached samples."""
|
||||
with self.lock:
|
||||
now = self.rospy.Time.now().to_sec()
|
||||
previous = getattr(self, "_last_ros_time", None)
|
||||
epoch = getattr(self, "_ros_epoch", 0)
|
||||
if not math.isfinite(now) or (previous is not None and now < previous):
|
||||
epoch += 1
|
||||
self.previous_odom_stamp = None
|
||||
self._ros_epoch, self._last_ros_time = epoch, now
|
||||
return now, epoch
|
||||
|
||||
def _source_metadata(self, stamp):
|
||||
now, epoch = self._observe_ros_clock()
|
||||
valid = (isinstance(stamp, (int, float)) and math.isfinite(stamp) and stamp > 0
|
||||
and math.isfinite(now) and 0 <= now-stamp <= self.max_age)
|
||||
return {"source_stamp": stamp, "source_epoch": epoch, "source_fresh": valid}
|
||||
|
||||
def source_is_fresh(self, sample):
|
||||
now, epoch = self._observe_ros_clock()
|
||||
stamp = sample.get("source_stamp")
|
||||
return (sample.get("source_fresh") is True and sample.get("source_epoch") == epoch
|
||||
and isinstance(stamp, (int, float)) and math.isfinite(stamp) and stamp > 0
|
||||
and math.isfinite(now) and 0 <= now-stamp <= self.max_age)
|
||||
|
||||
def _odom(self, msg):
|
||||
v, w = msg.twist.twist.linear, msg.twist.twist.angular
|
||||
stamp = msg.header.stamp.to_sec()
|
||||
with self.lock:
|
||||
self.sequence += 1
|
||||
metadata = self._source_metadata(stamp)
|
||||
metadata["source_fresh"] = metadata["source_fresh"] and (self.previous_odom_stamp is None or stamp > self.previous_odom_stamp)
|
||||
self.odom = {"sequence": self.sequence, "received_at": time.monotonic(),
|
||||
**metadata,
|
||||
"linear": math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z),
|
||||
"angular": math.sqrt(w.x*w.x + w.y*w.y + w.z*w.z)}
|
||||
self.previous_odom_stamp = stamp
|
||||
self.odom_queue.append(copy.deepcopy(self.odom))
|
||||
# Overflow loses continuity; the sequence gap resets the core's window.
|
||||
if len(self.odom_queue) > 4096: self.odom_queue = self.odom_queue[-1:]
|
||||
|
||||
def _pose(self, msg):
|
||||
p, q = msg.pose.position, msg.pose.orientation
|
||||
with self.lock:
|
||||
self.pose = {"received_at": time.monotonic(), **self._source_metadata(msg.header.stamp.to_sec()),
|
||||
"value": {"frame_id": msg.header.frame_id, "position": dict(x=p.x, y=p.y, z=p.z),
|
||||
"orientation": dict(x=q.x, y=q.y, z=q.z, w=q.w)}}
|
||||
|
||||
def _health(self, msg):
|
||||
import json
|
||||
try:
|
||||
data = json.loads(msg.data)
|
||||
metadata = self._source_metadata(float(data["stamp"]))
|
||||
valid = data.get("ready") is True and data.get("map_id") == self.map_id and metadata["source_fresh"]
|
||||
value = {"ready": valid, "map_id": data.get("map_id"), **metadata,
|
||||
"reason": str(data.get("reason", "")), "received_at": time.monotonic()}
|
||||
except (ValueError, TypeError, KeyError):
|
||||
value = {"ready": False, "map_id": self.map_id, "source_fresh": False,
|
||||
"reason": "invalid health monitor payload", "received_at": time.monotonic()}
|
||||
with self.lock: self.health_value = value
|
||||
|
||||
def health(self):
|
||||
with self.lock:
|
||||
out = copy.deepcopy(self.health_value)
|
||||
out["source_fresh"] = self.source_is_fresh(out)
|
||||
if not out["source_fresh"]: out.update(ready=False, reason="health source timestamp stale or invalidated")
|
||||
if not self.connected: out.update(ready=False, reason="move_base server unavailable")
|
||||
return out
|
||||
|
||||
def send(self, goal):
|
||||
with self.lock:
|
||||
self.current_id, self.state = goal["goal_id"], "PENDING"
|
||||
msg = self.MoveBaseGoal()
|
||||
msg.target_pose.header.frame_id = "map"
|
||||
msg.target_pose.header.stamp = self.rospy.Time.now()
|
||||
for key, value in goal["target_pose"]["position"].items(): setattr(msg.target_pose.pose.position, key, value)
|
||||
for key, value in goal["target_pose"]["orientation"].items(): setattr(msg.target_pose.pose.orientation, key, value)
|
||||
goal_id = goal["goal_id"]
|
||||
def active():
|
||||
with self.lock:
|
||||
if self.current_id == goal_id: self.state = "ACTIVE"
|
||||
def done(status, result):
|
||||
with self.lock:
|
||||
if self.current_id == goal_id: self.state = self.status_names.get(status, "UNKNOWN")
|
||||
# send_goal is asynchronous; return indicates local dispatch, not server acceptance.
|
||||
self.client.send_goal(msg, done_cb=done, active_cb=active)
|
||||
return "ACCEPTED"
|
||||
|
||||
def cancel(self, goal_id):
|
||||
with self.lock:
|
||||
if goal_id != self.current_id: raise RuntimeError("cannot correlate ROS1 goal after restart")
|
||||
self.client.cancel_goal()
|
||||
|
||||
def snapshot(self, goal_id):
|
||||
with self.lock:
|
||||
if goal_id != self.current_id: return {"controller_state": "UNKNOWN"}
|
||||
# get_state also surfaces LOST without waiting for a done callback.
|
||||
state = self.status_names.get(self.client.get_state(), self.state)
|
||||
out = {"controller_state": state, "odom": copy.deepcopy(self.odom), "pose": copy.deepcopy(self.pose),
|
||||
"odom_samples": copy.deepcopy(self.odom_queue)}
|
||||
for sample in [out["odom"], out["pose"]] + out["odom_samples"]:
|
||||
if isinstance(sample, dict): sample["source_fresh"] = self.source_is_fresh(sample)
|
||||
self.odom_queue.clear()
|
||||
return out
|
||||
@@ -0,0 +1,20 @@
|
||||
"""Navigation-owned immutable, versioned exact-match lookup. No fuzzy guesses."""
|
||||
import copy,math
|
||||
class Catalog:
|
||||
def __init__(self,site):
|
||||
self.site=copy.deepcopy(site)
|
||||
if type(site.get('registry_version')) is not int or site['registry_version']<=0:raise ValueError('registry version required')
|
||||
def resolve(self,kind,reference,version,*,shelf='',side='',column='',tier=''):
|
||||
if type(version) is not int or version!=self.site['registry_version']:raise ValueError('REGISTRY_VERSION_MISMATCH')
|
||||
if kind=='LOCATION':location=reference
|
||||
elif kind=='OBJECT':location=self.site.get('object_locations',{}).get(reference)
|
||||
elif kind=='CELL':
|
||||
if any(not isinstance(x,str) or not x or '/' in x for x in (shelf,side,column,tier)):raise ValueError('INVALID_CELL')
|
||||
location=self.site.get('cell_locations',{}).get('/'.join((shelf,side,column,tier)))
|
||||
else:raise ValueError('UNKNOWN_LOOKUP_KIND')
|
||||
pose=self.site.get('locations',{}).get(location)
|
||||
if not isinstance(pose,dict) or pose.get('frame_id')!='map':raise ValueError('NOT_FOUND')
|
||||
for k in ('x','y','z','qx','qy','qz','qw'):
|
||||
if type(pose.get(k)) not in (int,float) or not math.isfinite(pose[k]):raise ValueError('INVALID_POSE')
|
||||
if abs(sum(pose[k]**2 for k in ('qx','qy','qz','qw'))-1)>.001:raise ValueError('INVALID_QUATERNION')
|
||||
return dict(location_id=location,pose=copy.deepcopy(pose),registry_version=version)
|
||||
@@ -0,0 +1,288 @@
|
||||
"""Persistent, single robot navigation resource ownership and stop verification."""
|
||||
from dataclasses import dataclass, asdict
|
||||
import copy
|
||||
import fcntl
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import sqlite3
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from .backends import TERMINAL_CONTROLLERS
|
||||
|
||||
|
||||
class GatewayError(Exception):
|
||||
def __init__(self, message, http_status=400):
|
||||
super().__init__(message)
|
||||
self.http_status = http_status
|
||||
|
||||
|
||||
def number(value, name, positive=False):
|
||||
if isinstance(value, bool) or not isinstance(value, (float, int)) or not math.isfinite(value):
|
||||
raise GatewayError(name + " must be finite numeric")
|
||||
if positive and value <= 0: raise GatewayError(name + " must be positive")
|
||||
return float(value)
|
||||
|
||||
|
||||
def validate_pose(pose):
|
||||
if not isinstance(pose, dict) or set(pose) != {"frame_id", "position", "orientation"} or pose["frame_id"] != "map":
|
||||
raise GatewayError("target_pose must use map frame and exact pose fields")
|
||||
for group, keys in (("position", {"x", "y", "z"}), ("orientation", {"x", "y", "z", "w"})):
|
||||
if not isinstance(pose[group], dict) or set(pose[group]) != keys: raise GatewayError("invalid " + group)
|
||||
for key in keys: number(pose[group][key], group + "." + key)
|
||||
norm = sum(v*v for v in pose["orientation"].values())
|
||||
if abs(norm - 1.0) > 1e-3: raise GatewayError("quaternion must be normalized")
|
||||
|
||||
|
||||
def validate_goal(body):
|
||||
required = {"goal_id", "trace", "map_id", "target_pose", "position_tolerance", "yaw_tolerance", "timeout_sec"}
|
||||
if not isinstance(body, dict) or set(body) != required: raise GatewayError("invalid or unknown goal fields")
|
||||
try:
|
||||
if str(uuid.UUID(body["goal_id"])) != body["goal_id"]: raise ValueError()
|
||||
except (ValueError, TypeError, AttributeError): raise GatewayError("goal_id must be canonical UUID")
|
||||
if not isinstance(body["map_id"], str) or not body["map_id"] or len(body["map_id"]) > 256: raise GatewayError("invalid map_id")
|
||||
trace = body["trace"]
|
||||
if not isinstance(trace, dict) or not {"task_id", "subtask_id", "attempt"}.issubset(trace): raise GatewayError("trace identifiers required")
|
||||
allowed_trace = {"task_id", "subtask_id", "attempt", "task_revision", "plan_version", "run_id", "execution_generation"}
|
||||
if set(trace) - allowed_trace: raise GatewayError("unknown trace fields")
|
||||
for key in ("task_id", "subtask_id"):
|
||||
if not isinstance(trace[key], str) or not trace[key] or len(trace[key]) > 256: raise GatewayError("invalid trace " + key)
|
||||
for key, value in trace.items():
|
||||
if key in {"attempt", "task_revision", "plan_version", "execution_generation"}:
|
||||
if type(value) is not int or value < 0: raise GatewayError("invalid trace counter")
|
||||
elif not isinstance(value, str) or len(value) > 256: raise GatewayError("invalid trace text")
|
||||
validate_pose(body["target_pose"])
|
||||
for key in ("position_tolerance", "yaw_tolerance", "timeout_sec"): number(body[key], key, positive=True)
|
||||
if body["yaw_tolerance"] > math.pi: raise GatewayError("yaw_tolerance is radians and must be <= pi")
|
||||
return copy.deepcopy(body)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SafetyConfig:
|
||||
odom_max_age_sec: float
|
||||
stationary_window_sec: float
|
||||
linear_stopped_mps: float
|
||||
angular_stopped_radps: float
|
||||
pose_max_age_sec: float
|
||||
readiness_max_age_sec: float
|
||||
stop_wait_timeout_sec: float
|
||||
|
||||
def __post_init__(self):
|
||||
for key, value in asdict(self).items(): number(value, key, positive=True)
|
||||
|
||||
|
||||
def pose_errors(goal_pose, actual_pose):
|
||||
validate_pose(actual_pose)
|
||||
p, a = goal_pose["position"], actual_pose["position"]
|
||||
position = math.hypot(p["x"] - a["x"], p["y"] - a["y"])
|
||||
def yaw(pose):
|
||||
q = pose["orientation"]
|
||||
return math.atan2(2*(q["w"]*q["z"]+q["x"]*q["y"]), 1-2*(q["y"]**2+q["z"]**2))
|
||||
return position, math.remainder(yaw(goal_pose)-yaw(actual_pose), 2*math.pi)
|
||||
|
||||
|
||||
class Gateway:
|
||||
def __init__(self, journal_path, backend, config, clock=time.monotonic):
|
||||
self.backend, self.config, self.clock = backend, config, clock
|
||||
self.lock, self.records, self.closed = threading.RLock(), {}, False
|
||||
os.makedirs(os.path.dirname(os.path.abspath(journal_path)), exist_ok=True)
|
||||
self.lockfile = open(journal_path + ".lock", "a+")
|
||||
try: fcntl.flock(self.lockfile.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB)
|
||||
except OSError:
|
||||
self.lockfile.close()
|
||||
raise RuntimeError("another gateway owns this journal")
|
||||
self.db = sqlite3.connect(journal_path, check_same_thread=False)
|
||||
os.chmod(journal_path, 0o600)
|
||||
self.db.execute("PRAGMA journal_mode=WAL")
|
||||
self.db.execute("PRAGMA synchronous=FULL")
|
||||
self.db.execute("CREATE TABLE IF NOT EXISTS goals (goal_id TEXT PRIMARY KEY, request_hash TEXT NOT NULL, record TEXT NOT NULL)")
|
||||
for _, _, raw in self.db.execute("SELECT goal_id, request_hash, record FROM goals"):
|
||||
record = json.loads(raw)
|
||||
if record["status"] != "TERMINAL" or record["stop_state"] != "CONFIRMED":
|
||||
record.update(status="STOP_UNKNOWN", stop_state="UNKNOWN", quarantined=True,
|
||||
message="process restart: manual reconciliation required")
|
||||
self.records[record["goal_id"]] = record
|
||||
self._save(record)
|
||||
|
||||
def _save(self, record):
|
||||
raw = json.dumps(record, sort_keys=True, allow_nan=False)
|
||||
with self.db:
|
||||
self.db.execute("INSERT INTO goals VALUES (?, ?, ?) ON CONFLICT(goal_id) DO UPDATE SET record=excluded.record",
|
||||
(record["goal_id"], record["request_hash"], raw))
|
||||
|
||||
def _public(self, record):
|
||||
out = {key: copy.deepcopy(value) for key, value in record.items() if not key.startswith("_")}
|
||||
out.pop("request", None)
|
||||
if not record["quarantined"] and record["status"] != "TERMINAL": out["elapsed"] = max(0, self.clock()-record["_started"])
|
||||
return out
|
||||
|
||||
def _record(self, goal_id):
|
||||
if goal_id not in self.records: raise GatewayError("unknown goal UUID", 404)
|
||||
return self.records[goal_id]
|
||||
|
||||
def _occupied(self):
|
||||
return any(r["status"] != "TERMINAL" or r["stop_state"] != "CONFIRMED" for r in self.records.values())
|
||||
|
||||
def health(self):
|
||||
with self.lock:
|
||||
try: health = self.backend.health()
|
||||
except Exception: health = {}
|
||||
fresh = self._fresh(health, self.config.readiness_max_age_sec)
|
||||
ready = health.get("ready") is True and fresh and not self._occupied()
|
||||
return {"ready": ready, "map_id": health.get("map_id"), "stamp_monotonic": self.clock(),
|
||||
"reason": "motion resource occupied or quarantined" if self._occupied() else
|
||||
(health.get("reason", "") if fresh else "readiness missing or stale")}
|
||||
|
||||
def submit(self, body):
|
||||
goal = validate_goal(body)
|
||||
canonical = json.dumps(goal, sort_keys=True, separators=(",", ":"), allow_nan=False)
|
||||
digest = hashlib.sha256(canonical.encode()).hexdigest()
|
||||
with self.lock:
|
||||
if goal["goal_id"] in self.records:
|
||||
record = self._record(goal["goal_id"])
|
||||
if record["request_hash"] != digest: raise GatewayError("UUID already bound to different immutable request", 409)
|
||||
return self._public(record)
|
||||
if self._occupied(): raise GatewayError("motion resource occupied; manual reconciliation may be required", 409)
|
||||
health = self.health()
|
||||
if not health["ready"] or health["map_id"] != goal["map_id"]: raise GatewayError("backend not ready for requested map", 503)
|
||||
record = {"goal_id": goal["goal_id"], "request_hash": digest, "request": goal, "status": "SENDING",
|
||||
"outcome": None, "stop_state": "UNKNOWN", "controller_state": "UNKNOWN", "message": "dispatch pending",
|
||||
"sequence": 0, "elapsed": 0, "position_error": None, "yaw_error": None, "quarantined": False,
|
||||
"pose_valid": False, "current_pose": None, "final_pose": None,
|
||||
"stopped_at": None,
|
||||
"_started": self.clock(), "_cancel_sent": False, "_cancel_outcome": None, "_cancel_at": None,
|
||||
"_terminal_at": None, "_stationary_since": None, "_last_odom_at": None, "_last_odom_sequence": None}
|
||||
self.records[goal["goal_id"]] = record
|
||||
self._save(record) # Commit ownership BEFORE any potentially side-effecting backend send.
|
||||
try: acceptance = self.backend.send(goal)
|
||||
except Exception: acceptance = "UNKNOWN"
|
||||
if acceptance == "ACCEPTED": record.update(status="ACTIVE", message="dispatched; server acceptance observed asynchronously")
|
||||
elif acceptance == "REJECTED": record.update(status="ACTIVE", controller_state="REJECTED", outcome="REJECTED", message="rejected; stop evidence pending")
|
||||
else: record.update(status="STOP_UNKNOWN", quarantined=True, message="dispatch outcome unknown: reconcile original goal; never retry")
|
||||
self._save(record)
|
||||
if record["quarantined"]: self.cancel(goal["goal_id"], "FAILED")
|
||||
return self._public(record)
|
||||
|
||||
def get(self, goal_id):
|
||||
with self.lock: return self._public(self._record(goal_id))
|
||||
|
||||
def cancel(self, goal_id, outcome="CANCELED"):
|
||||
with self.lock:
|
||||
r = self._record(goal_id)
|
||||
if r["status"] == "TERMINAL" and r["stop_state"] == "CONFIRMED": return self._public(r)
|
||||
if not r["_cancel_sent"]:
|
||||
r.update(_cancel_sent=True, _cancel_outcome=outcome, _cancel_at=self.clock())
|
||||
if not r["quarantined"]: r["status"] = "CANCEL_REQUESTED"
|
||||
self._save(r) # A crash here must NOT repeat a possibly side-effecting cancel.
|
||||
try: self.backend.cancel(goal_id)
|
||||
except Exception: r.update(status="STOP_UNKNOWN", message="cancel delivery unknown; stop not confirmed")
|
||||
self._save(r)
|
||||
return self._public(r)
|
||||
|
||||
def _fresh(self, sample, maximum):
|
||||
if not isinstance(sample, dict) or sample.get("source_fresh") is not True: return False
|
||||
try:
|
||||
if not self.backend.source_is_fresh(sample): return False
|
||||
except (AttributeError, KeyError, TypeError, ValueError): return False
|
||||
stamp = sample.get("received_at")
|
||||
return isinstance(stamp, (int, float)) and math.isfinite(stamp) and 0 <= self.clock()-stamp <= maximum
|
||||
|
||||
def poll(self, goal_id):
|
||||
with self.lock:
|
||||
r = self._record(goal_id)
|
||||
if r["quarantined"] or (r["status"] == "TERMINAL" and r["stop_state"] == "CONFIRMED"): return self._public(r)
|
||||
if self.clock()-r["_started"] >= r["request"]["timeout_sec"] and not r["_cancel_sent"]:
|
||||
self.cancel(goal_id, "TIMED_OUT")
|
||||
if not r["_cancel_sent"]:
|
||||
try: health = self.backend.health()
|
||||
except Exception: health = {}
|
||||
if not self._fresh(health, self.config.readiness_max_age_sec) or health.get("ready") is not True or health.get("map_id") != r["request"]["map_id"]:
|
||||
self.cancel(goal_id, "FAILED")
|
||||
r["message"] = "readiness lost during execution; stop requested"
|
||||
try: snapshot = self.backend.snapshot(goal_id)
|
||||
except Exception: snapshot = {"controller_state": "UNKNOWN"}
|
||||
state = snapshot.get("controller_state", "UNKNOWN")
|
||||
r.update(controller_state=state, sequence=r["sequence"]+1, elapsed=max(0, self.clock()-r["_started"]))
|
||||
r.update(pose_valid=False, current_pose=None)
|
||||
pose_sample = snapshot.get("pose")
|
||||
if self._fresh(pose_sample, self.config.pose_max_age_sec):
|
||||
try:
|
||||
p_error, y_error = pose_errors(r["request"]["target_pose"], pose_sample["value"])
|
||||
r.update(pose_valid=True, current_pose=copy.deepcopy(pose_sample["value"]),
|
||||
position_error=p_error, yaw_error=y_error)
|
||||
except (GatewayError, KeyError, TypeError): pass
|
||||
if not r["pose_valid"]: r.update(position_error=None, yaw_error=None)
|
||||
if state in {"UNKNOWN", "LOST"}:
|
||||
if not r["_cancel_sent"]: self.cancel(goal_id, "FAILED")
|
||||
r.update(status="STOP_UNKNOWN", message="controller state unknown; resource remains locked", _stationary_since=None)
|
||||
elif state in TERMINAL_CONTROLLERS:
|
||||
if r["_terminal_at"] is None: r["_terminal_at"] = self.clock()
|
||||
stationary = False
|
||||
# Consume every received sample, so motion between HTTP polls cannot disappear.
|
||||
samples = snapshot.get("odom_samples") or [snapshot.get("odom")]
|
||||
for odom in samples:
|
||||
stationary = False
|
||||
if self._fresh(odom, self.config.odom_max_age_sec):
|
||||
try:
|
||||
linear = number(odom["linear"], "measured linear speed")
|
||||
angular = number(odom["angular"], "measured angular speed")
|
||||
stationary = abs(linear) <= self.config.linear_stopped_mps and abs(angular) <= self.config.angular_stopped_radps
|
||||
except (GatewayError, KeyError): pass
|
||||
if not stationary:
|
||||
r["_stationary_since"] = None
|
||||
if not isinstance(odom, dict): continue
|
||||
seq, stamp = odom.get("sequence"), odom.get("received_at")
|
||||
if type(seq) is not int or seq < 1:
|
||||
r["_stationary_since"] = None
|
||||
continue
|
||||
if seq == r["_last_odom_sequence"]: continue # Re-reading one sample never advances the window.
|
||||
if not isinstance(stamp, (int, float)) or not math.isfinite(stamp):
|
||||
r["_stationary_since"] = None
|
||||
continue
|
||||
gap = (r["_last_odom_at"] is None or stamp-r["_last_odom_at"] > self.config.odom_max_age_sec
|
||||
or stamp <= r["_last_odom_at"] or (r["_last_odom_sequence"] is not None and seq != r["_last_odom_sequence"]+1))
|
||||
if stamp < r["_terminal_at"]: r["_stationary_since"] = None
|
||||
elif stationary and (r["_stationary_since"] is None or gap): r["_stationary_since"] = stamp
|
||||
r["_last_odom_at"], r["_last_odom_sequence"] = stamp, seq
|
||||
if stationary and r["_stationary_since"] is not None and r["_last_odom_at"]-r["_stationary_since"] >= self.config.stationary_window_sec:
|
||||
outcome = r["_cancel_outcome"] or ({"SUCCEEDED": "COMPLETED", "ARRIVED": "COMPLETED",
|
||||
"REJECTED": "REJECTED", "PREEMPTED": "CANCELED", "RECALLED": "CANCELED"}.get(state, "FAILED"))
|
||||
message = "controller terminal and measured stationary window confirmed"
|
||||
if outcome == "COMPLETED":
|
||||
pose = snapshot.get("pose")
|
||||
try:
|
||||
if not self._fresh(pose, self.config.pose_max_age_sec): raise GatewayError("pose stale")
|
||||
p_err, y_err = pose_errors(r["request"]["target_pose"], pose["value"])
|
||||
r.update(position_error=p_err, yaw_error=y_err)
|
||||
if p_err > r["request"]["position_tolerance"] or abs(y_err) > r["request"]["yaw_tolerance"]: raise GatewayError("pose outside tolerance")
|
||||
except (GatewayError, KeyError, TypeError): outcome, message = "FAILED", "terminal success failed fresh pose/tolerance verification"
|
||||
# Preserve the original last physical odom observation that closed
|
||||
# the stationary window. Never timestamp cached results at query time.
|
||||
proof_stamp = odom.get("source_stamp")
|
||||
if not isinstance(proof_stamp, (int, float)) or not math.isfinite(proof_stamp) or proof_stamp <= 0:
|
||||
r.update(status="STOP_UNKNOWN", message="stationary proof lacks original source timestamp")
|
||||
self._save(r)
|
||||
return self._public(r)
|
||||
r.update(status="TERMINAL", outcome=outcome, stop_state="CONFIRMED", message=message,
|
||||
stopped_at=proof_stamp)
|
||||
if r["pose_valid"]: r["final_pose"] = copy.deepcopy(r["current_pose"])
|
||||
else:
|
||||
r.update(_terminal_at=None, _stationary_since=None)
|
||||
if r["_cancel_at"] is not None and self.clock()-r["_cancel_at"] >= self.config.stop_wait_timeout_sec and r["stop_state"] != "CONFIRMED":
|
||||
r.update(status="STOP_UNKNOWN", message="stop confirmation deadline exceeded; resource remains locked")
|
||||
self._save(r)
|
||||
return self._public(r)
|
||||
|
||||
def poll_all(self):
|
||||
with self.lock:
|
||||
for goal_id in list(self.records): self.poll(goal_id)
|
||||
|
||||
def close(self):
|
||||
with self.lock:
|
||||
if not self.closed:
|
||||
self.db.close()
|
||||
fcntl.flock(self.lockfile.fileno(), fcntl.LOCK_UN)
|
||||
self.lockfile.close()
|
||||
self.closed = True
|
||||
@@ -0,0 +1,620 @@
|
||||
"""ROS 2 Navigate Action -> authenticated navigation gateway.
|
||||
|
||||
The HTTP/session layer uses only the Python standard library. ROS imports are
|
||||
lazy so transport behavior can be exercised without a ROS installation.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import http.client
|
||||
import ipaddress
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any, Mapping
|
||||
from urllib.parse import urlsplit
|
||||
|
||||
|
||||
_STATES = {'SENDING', 'ACTIVE', 'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'}
|
||||
_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED'}
|
||||
_CONTROLLER_TERMINALS = {'ARRIVED', 'SUCCEEDED', 'ABORTED', 'PREEMPTED', 'RECALLED', 'REJECTED'}
|
||||
_TRACE_FIELDS = ('task_id', 'subtask_id', 'attempt', 'task_revision', 'plan_version',
|
||||
'run_id', 'execution_generation')
|
||||
_MAX_RESPONSE_BYTES = 65536
|
||||
|
||||
|
||||
class GatewayError(RuntimeError):
|
||||
"""A response cannot establish the state of the physical goal."""
|
||||
|
||||
|
||||
def _positive(value: Any, name: str) -> float:
|
||||
if isinstance(value, bool) or not isinstance(value, (int, float)):
|
||||
raise ValueError(f'{name} must be a finite positive number')
|
||||
result = float(value)
|
||||
if not math.isfinite(result) or result <= 0:
|
||||
raise ValueError(f'{name} must be a finite positive number')
|
||||
return result
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ProxyConfig:
|
||||
token: str = field(repr=False)
|
||||
connect_timeout_sec: float
|
||||
read_timeout_sec: float
|
||||
request_timeout_sec: float
|
||||
poll_interval_sec: float
|
||||
readiness_max_age_sec: float
|
||||
feedback_silence_timeout_sec: float
|
||||
endpoint: str = 'http://127.0.0.1:8766'
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if not isinstance(self.token, str) or len(self.token) < 16 or any(c.isspace() for c in self.token):
|
||||
raise ValueError('A bearer token of at least 16 characters without whitespace is required')
|
||||
for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec',
|
||||
'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'):
|
||||
_positive(getattr(self, name), name)
|
||||
parsed = urlsplit(self.endpoint)
|
||||
if (parsed.scheme not in {'http', 'https'} or not parsed.hostname or
|
||||
parsed.username is not None or parsed.password is not None or
|
||||
parsed.path not in {'', '/'} or parsed.query or parsed.fragment):
|
||||
raise ValueError('endpoint must be an http(s) origin without credentials or path')
|
||||
# DNS lookup has no socket deadline in stdlib. Use an explicit deployment
|
||||
# IP; localhost is normalized without invoking the resolver.
|
||||
if parsed.hostname != 'localhost':
|
||||
try:
|
||||
ipaddress.ip_address(parsed.hostname)
|
||||
except ValueError as exc:
|
||||
raise ValueError('endpoint must use an IP address or localhost') from exc
|
||||
try:
|
||||
parsed.port
|
||||
except ValueError as exc:
|
||||
raise ValueError('invalid endpoint port') from exc
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GatewaySnapshot:
|
||||
goal_id: str
|
||||
status: str
|
||||
outcome: str | None
|
||||
stop_state: str
|
||||
controller_state: str
|
||||
message: str
|
||||
position_error: float | None
|
||||
yaw_error: float | None
|
||||
sequence: int
|
||||
elapsed: float
|
||||
pose_valid: bool = False
|
||||
current_pose: dict[str, Any] | None = None
|
||||
final_pose: dict[str, Any] | None = None
|
||||
stopped_at: float | None = None
|
||||
|
||||
@classmethod
|
||||
def parse(cls, data: Mapping[str, Any], goal_id: str) -> 'GatewaySnapshot':
|
||||
try:
|
||||
required = {name: data[name] for name in cls.__dataclass_fields__
|
||||
if name not in {'pose_valid', 'current_pose', 'final_pose', 'stopped_at'}}
|
||||
snapshot = cls(**required, pose_valid=data.get('pose_valid', False),
|
||||
current_pose=validate_observed_pose(data.get('current_pose')),
|
||||
final_pose=validate_observed_pose(data.get('final_pose')),
|
||||
stopped_at=data.get('stopped_at'))
|
||||
except (TypeError, KeyError) as exc:
|
||||
raise GatewayError('Gateway snapshot is missing required fields') from exc
|
||||
stop_time_parts = None if snapshot.stopped_at is None else _ros_time_parts(snapshot.stopped_at)
|
||||
if type(snapshot.pose_valid) is not bool:
|
||||
raise GatewayError('Gateway pose_valid must be boolean')
|
||||
if snapshot.pose_valid and snapshot.current_pose is None and snapshot.final_pose is None:
|
||||
raise GatewayError('Gateway marks pose valid without an observed pose')
|
||||
if snapshot.goal_id != goal_id:
|
||||
raise GatewayError('Gateway returned a different goal UUID')
|
||||
if snapshot.status not in _STATES or snapshot.outcome not in _OUTCOMES | {None}:
|
||||
raise GatewayError('Gateway returned an unknown status or outcome')
|
||||
if snapshot.stop_state not in {'UNKNOWN', 'CONFIRMED'}:
|
||||
raise GatewayError('Gateway returned an unknown stop state')
|
||||
if not isinstance(snapshot.controller_state, str) or not isinstance(snapshot.message, str):
|
||||
raise GatewayError('Gateway returned malformed text fields')
|
||||
if type(snapshot.sequence) is not int or not 0 <= snapshot.sequence <= 0xffffffff:
|
||||
raise GatewayError('Gateway sequence must fit Navigate uint32')
|
||||
for name in ('elapsed', 'position_error', 'yaw_error'):
|
||||
value = getattr(snapshot, name)
|
||||
if value is None and name != 'elapsed':
|
||||
continue
|
||||
if (isinstance(value, bool) or not isinstance(value, (float, int)) or
|
||||
not math.isfinite(value) or (name != 'yaw_error' and value < 0) or
|
||||
(name == 'yaw_error' and abs(value) > math.pi)):
|
||||
raise GatewayError(f'Gateway {name} is invalid')
|
||||
if snapshot.status == 'TERMINAL' and snapshot.outcome is None:
|
||||
raise GatewayError('Terminal gateway snapshot has no outcome')
|
||||
if snapshot.status == 'TERMINAL' and snapshot.stop_state == 'CONFIRMED':
|
||||
if stop_time_parts is None or stop_time_parts == (0, 0):
|
||||
raise GatewayError('Confirmed result requires a positive original ROS stop timestamp')
|
||||
if snapshot.controller_state not in _CONTROLLER_TERMINALS:
|
||||
raise GatewayError('Confirmed result lacks a native controller terminal')
|
||||
if snapshot.outcome == 'COMPLETED' and snapshot.controller_state not in {'ARRIVED', 'SUCCEEDED'}:
|
||||
raise GatewayError('COMPLETED conflicts with native controller state')
|
||||
return snapshot
|
||||
|
||||
@property
|
||||
def is_terminal(self) -> bool:
|
||||
return self.status == 'TERMINAL' and self.stop_state == 'CONFIRMED'
|
||||
|
||||
|
||||
def _ros_time_parts(source_seconds: Any) -> tuple[int, int]:
|
||||
if (isinstance(source_seconds, bool) or not isinstance(source_seconds, (int, float)) or
|
||||
not math.isfinite(source_seconds) or source_seconds < 0 or
|
||||
source_seconds >= 2147483648):
|
||||
raise GatewayError('Stop timestamp must fit a nonnegative ROS int32 seconds value')
|
||||
seconds = math.floor(source_seconds)
|
||||
nanoseconds = round((source_seconds - seconds) * 1_000_000_000)
|
||||
if nanoseconds >= 1_000_000_000:
|
||||
seconds += 1
|
||||
nanoseconds -= 1_000_000_000
|
||||
if seconds > 2147483647:
|
||||
raise GatewayError('Rounded stop timestamp exceeds ROS int32 seconds')
|
||||
return seconds, nanoseconds
|
||||
|
||||
|
||||
def assign_ros_time(destination: Any, source_seconds: float) -> None:
|
||||
"""Copy the original gateway evidence time, never callback receipt time."""
|
||||
destination.sec, destination.nanosec = _ros_time_parts(source_seconds)
|
||||
|
||||
|
||||
def validate_observed_pose(pose: Any) -> dict[str, Any] | None:
|
||||
"""Validate an actual map-frame observation without normalizing bad data."""
|
||||
if pose is None:
|
||||
return None
|
||||
if not isinstance(pose, dict) or set(pose) != {'frame_id', 'position', 'orientation'}:
|
||||
raise GatewayError('Observed pose must use the gateway map-pose schema')
|
||||
if pose['frame_id'] != 'map':
|
||||
raise GatewayError('Observed pose must be in the map frame')
|
||||
for name, axes in (('position', {'x', 'y', 'z'}), ('orientation', {'x', 'y', 'z', 'w'})):
|
||||
coordinates = pose[name]
|
||||
if not isinstance(coordinates, dict) or set(coordinates) != axes:
|
||||
raise GatewayError('Observed pose coordinates are incomplete')
|
||||
if any(isinstance(v, bool) or not isinstance(v, (float, int)) or
|
||||
not math.isfinite(v) for v in coordinates.values()):
|
||||
raise GatewayError('Observed pose coordinates must be finite')
|
||||
if abs(sum(value * value for value in pose['orientation'].values()) - 1.0) > 1e-3:
|
||||
raise GatewayError('Observed pose quaternion must be normalized')
|
||||
return json.loads(json.dumps(pose, allow_nan=False))
|
||||
|
||||
|
||||
def assign_ros_pose(destination: Any, source: Mapping[str, Any]) -> None:
|
||||
"""Copy validated geometry; unavailable source timestamp remains unset."""
|
||||
destination.header.frame_id = source['frame_id']
|
||||
for name, axes in (('position', ('x', 'y', 'z')), ('orientation', ('x', 'y', 'z', 'w'))):
|
||||
for axis in axes:
|
||||
setattr(getattr(destination.pose, name), axis, float(source[name][axis]))
|
||||
|
||||
|
||||
class GatewayClient:
|
||||
"""One fresh bounded connection per request; no redirect or proxy handling."""
|
||||
|
||||
def __init__(self, config: ProxyConfig):
|
||||
self.config = config
|
||||
parsed = urlsplit(config.endpoint)
|
||||
self._host = '127.0.0.1' if parsed.hostname == 'localhost' else parsed.hostname
|
||||
self._port = parsed.port
|
||||
self._connection_type = (http.client.HTTPSConnection if parsed.scheme == 'https'
|
||||
else http.client.HTTPConnection)
|
||||
|
||||
def _request(self, method: str, path: str,
|
||||
body: Mapping[str, Any] | None = None) -> dict[str, Any]:
|
||||
raw = None if body is None else json.dumps(body, allow_nan=False, sort_keys=True,
|
||||
separators=(',', ':')).encode('utf-8')
|
||||
config = self.config
|
||||
connection = self._connection_type(self._host, self._port,
|
||||
timeout=min(config.connect_timeout_sec, config.request_timeout_sec))
|
||||
expired = threading.Event()
|
||||
connected_socket = None
|
||||
|
||||
def expire() -> None:
|
||||
expired.set()
|
||||
active_socket = connected_socket or connection.sock
|
||||
if active_socket is not None:
|
||||
try:
|
||||
active_socket.shutdown(socket.SHUT_RDWR)
|
||||
except OSError:
|
||||
pass
|
||||
active_socket.close()
|
||||
|
||||
deadline = time.monotonic() + config.request_timeout_sec
|
||||
watchdog = threading.Timer(config.request_timeout_sec, expire)
|
||||
watchdog.daemon = True
|
||||
watchdog.start()
|
||||
try:
|
||||
connection.connect()
|
||||
connected_socket = connection.sock
|
||||
if expired.is_set() or connection.sock is None:
|
||||
raise GatewayError('Gateway request deadline exceeded')
|
||||
connection.sock.settimeout(min(config.read_timeout_sec,
|
||||
max(0.001, deadline - time.monotonic())))
|
||||
connection.request(method, path, raw, {
|
||||
'Authorization': f'Bearer {config.token}',
|
||||
'Accept': 'application/json',
|
||||
'Content-Type': 'application/json',
|
||||
'Connection': 'close',
|
||||
})
|
||||
response = connection.getresponse()
|
||||
# The response may own the socket once Connection: close is seen.
|
||||
# The watchdog retains its original reference below through this read.
|
||||
payload = response.read(_MAX_RESPONSE_BYTES + 1)
|
||||
if expired.is_set() or time.monotonic() > deadline:
|
||||
raise GatewayError('Gateway request deadline exceeded')
|
||||
if len(payload) > _MAX_RESPONSE_BYTES:
|
||||
raise GatewayError('Gateway response exceeds size limit')
|
||||
if not 200 <= response.status < 300:
|
||||
raise GatewayError(f'Gateway HTTP status {response.status}; stop is unknown')
|
||||
data = json.loads(payload)
|
||||
if not isinstance(data, dict):
|
||||
raise GatewayError('Gateway response must be a JSON object')
|
||||
return data
|
||||
except (OSError, http.client.HTTPException, ValueError) as exc:
|
||||
# Do not log the token, endpoint credentials, or arbitrary response body.
|
||||
raise GatewayError(f'Gateway transport/protocol failure: {type(exc).__name__}') from exc
|
||||
finally:
|
||||
watchdog.cancel()
|
||||
connection.close()
|
||||
|
||||
def health(self) -> dict[str, Any]:
|
||||
health = self._request('GET', '/healthz')
|
||||
if type(health.get('ready')) is not bool or not isinstance(health.get('reason'), str):
|
||||
raise GatewayError('Gateway health response is malformed')
|
||||
return health
|
||||
|
||||
def submit(self, body: Mapping[str, Any]) -> GatewaySnapshot:
|
||||
return GatewaySnapshot.parse(self._request('POST', '/v1/goals', body), body['goal_id'])
|
||||
|
||||
def query(self, goal_id: str) -> GatewaySnapshot:
|
||||
_canonical_uuid(goal_id)
|
||||
return GatewaySnapshot.parse(self._request('GET', f'/v1/goals/{goal_id}'), goal_id)
|
||||
|
||||
def cancel(self, goal_id: str) -> GatewaySnapshot:
|
||||
_canonical_uuid(goal_id)
|
||||
return GatewaySnapshot.parse(self._request('POST', f'/v1/goals/{goal_id}/cancel', {}), goal_id)
|
||||
|
||||
|
||||
def _canonical_uuid(goal_id: str) -> None:
|
||||
if not isinstance(goal_id, str) or str(uuid.UUID(goal_id)) != goal_id:
|
||||
raise ValueError('goal_id must be a canonical UUID')
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SessionEvent:
|
||||
snapshot: GatewaySnapshot | None = None
|
||||
error: str | None = None
|
||||
terminal: bool = False
|
||||
|
||||
|
||||
class GoalSession:
|
||||
"""Background transport for one immutable attempt; cancel means intent only.
|
||||
|
||||
The caller owns physical resource locking. An error is always stop UNKNOWN.
|
||||
Coalescing to the latest event bounds memory while the ROS executor is busy.
|
||||
"""
|
||||
|
||||
def __init__(self, client: GatewayClient, body: Mapping[str, Any]):
|
||||
self.client = client
|
||||
self._body = json.dumps(body, allow_nan=False, sort_keys=True,
|
||||
separators=(',', ':')).encode('utf-8')
|
||||
frozen_body = json.loads(self._body)
|
||||
self._goal_id = frozen_body['goal_id']
|
||||
self._position_tolerance = _positive(frozen_body['position_tolerance'], 'position_tolerance')
|
||||
self._yaw_tolerance = _positive(frozen_body['yaw_tolerance'], 'yaw_tolerance')
|
||||
_canonical_uuid(self._goal_id)
|
||||
self._cancel = threading.Event()
|
||||
self._lock = threading.Lock()
|
||||
self._event: SessionEvent | None = None
|
||||
self._started = False
|
||||
|
||||
@property
|
||||
def goal_id(self) -> str:
|
||||
return self._goal_id
|
||||
|
||||
def start(self) -> None:
|
||||
with self._lock:
|
||||
if self._started:
|
||||
raise RuntimeError('One GoalSession can be started only once')
|
||||
self._started = True
|
||||
threading.Thread(target=self._run, name=f'nav-{self.goal_id}', daemon=True).start()
|
||||
|
||||
def request_cancel(self) -> None:
|
||||
self._cancel.set()
|
||||
|
||||
def drain_events(self) -> list[SessionEvent]:
|
||||
with self._lock:
|
||||
event, self._event = self._event, None
|
||||
return [] if event is None else [event]
|
||||
|
||||
def _publish(self, event: SessionEvent) -> None:
|
||||
with self._lock:
|
||||
self._event = event
|
||||
|
||||
def _run(self) -> None:
|
||||
cancel_sent = False
|
||||
last_sequence = -1
|
||||
last_progress = time.monotonic()
|
||||
try:
|
||||
# A submission acknowledgement, even one containing a terminal
|
||||
# snapshot, is not consumed as a physical completion result.
|
||||
self.client.submit(json.loads(self._body))
|
||||
while True:
|
||||
if self._cancel.is_set() and not cancel_sent:
|
||||
cancel_sent = True
|
||||
self.client.cancel(self.goal_id) # ACK only; never terminal.
|
||||
snapshot = self.client.query(self.goal_id)
|
||||
if snapshot.sequence > last_sequence:
|
||||
if snapshot.is_terminal and snapshot.outcome == 'COMPLETED':
|
||||
if (snapshot.position_error is None or snapshot.yaw_error is None or
|
||||
snapshot.position_error > self._position_tolerance or
|
||||
abs(snapshot.yaw_error) > self._yaw_tolerance):
|
||||
raise GatewayError('COMPLETED lacks matching pose tolerance evidence')
|
||||
last_sequence = snapshot.sequence
|
||||
last_progress = time.monotonic()
|
||||
self._publish(SessionEvent(snapshot=snapshot, terminal=snapshot.is_terminal))
|
||||
if snapshot.is_terminal:
|
||||
return
|
||||
if time.monotonic() - last_progress >= self.client.config.feedback_silence_timeout_sec:
|
||||
self._cancel.set()
|
||||
# Event.wait would spin forever once cancel is set. A private
|
||||
# event keeps polling bounded and leaves ROS callbacks unblocked.
|
||||
threading.Event().wait(self.client.config.poll_interval_sec)
|
||||
except Exception as exc:
|
||||
# A lost submit response may conceal an accepted moving goal. Send
|
||||
# one best-effort cancel under the same UUID before reporting UNKNOWN.
|
||||
if not cancel_sent:
|
||||
try:
|
||||
self.client.cancel(self.goal_id)
|
||||
except Exception:
|
||||
pass
|
||||
error = str(exc) if isinstance(exc, GatewayError) else type(exc).__name__
|
||||
self._publish(SessionEvent(error=error))
|
||||
|
||||
|
||||
def build_goal_body(goal: Any, goal_id: str, map_id: str) -> dict[str, Any]:
|
||||
"""Freeze the p13 Navigate goal into the Noetic gateway's explicit schema."""
|
||||
_canonical_uuid(goal_id)
|
||||
if not isinstance(map_id, str) or not map_id.strip():
|
||||
raise ValueError('map_id must be explicitly configured')
|
||||
if goal.target_pose.header.frame_id != 'map':
|
||||
raise ValueError('Navigate only accepts registered poses in the map frame')
|
||||
timeout = goal.timeout.sec + goal.timeout.nanosec / 1e9
|
||||
if not 0 <= goal.timeout.nanosec < 1_000_000_000:
|
||||
raise ValueError('timeout nanosec is not normalized')
|
||||
pose = goal.target_pose.pose
|
||||
body = {
|
||||
'goal_id': goal_id,
|
||||
'trace': {name: getattr(goal.trace, name) for name in _TRACE_FIELDS},
|
||||
'map_id': map_id,
|
||||
'target_pose': {
|
||||
'frame_id': 'map',
|
||||
'position': {axis: getattr(pose.position, axis) for axis in ('x', 'y', 'z')},
|
||||
'orientation': {axis: getattr(pose.orientation, axis) for axis in ('x', 'y', 'z', 'w')},
|
||||
},
|
||||
'position_tolerance': _positive(goal.position_tolerance, 'position_tolerance'),
|
||||
'yaw_tolerance': _positive(goal.orientation_tolerance, 'orientation_tolerance'),
|
||||
'timeout_sec': _positive(timeout, 'timeout'),
|
||||
}
|
||||
if not body['trace']['task_id'] or not body['trace']['subtask_id'] or body['trace']['attempt'] < 1:
|
||||
raise ValueError('TaskTrace task_id, subtask_id and attempt are required')
|
||||
values = list(body['target_pose']['position'].values()) + list(body['target_pose']['orientation'].values())
|
||||
if any(isinstance(v, bool) or not isinstance(v, (float, int)) or not math.isfinite(v) for v in values):
|
||||
raise ValueError('target pose must contain finite coordinates')
|
||||
return json.loads(json.dumps(body, allow_nan=False))
|
||||
|
||||
|
||||
def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
|
||||
"""Construct the ROS node; call only after rclpy.init().
|
||||
|
||||
ROS 2 Humble imports and action runtime require validation on the target.
|
||||
"""
|
||||
import rclpy
|
||||
from rclpy.action import ActionServer, CancelResponse, GoalResponse
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.node import Node
|
||||
from rclpy.task import Future
|
||||
from bt_skill_interfaces.action import Navigate
|
||||
from bt_skill_interfaces.msg import ExecutionResult
|
||||
|
||||
if not map_id or not action_name:
|
||||
raise ValueError('map_id and action_name must be explicitly configured')
|
||||
|
||||
class NavigateProxy(Node):
|
||||
def __init__(self):
|
||||
super().__init__('navigation_gateway_proxy')
|
||||
self._client = GatewayClient(config)
|
||||
self._guard = threading.Lock()
|
||||
self._ready = False
|
||||
self._health_at = 0.0
|
||||
self._reserved = False
|
||||
self._stop_unknown = False
|
||||
self._record = None
|
||||
self._closing = threading.Event()
|
||||
self._group = ReentrantCallbackGroup()
|
||||
self._server = ActionServer(self, Navigate, action_name,
|
||||
execute_callback=self._execute, goal_callback=self._accept,
|
||||
cancel_callback=self._cancel_goal, callback_group=self._group)
|
||||
self._pump_timer = self.create_timer(config.poll_interval_sec, self._pump)
|
||||
self._health_thread = threading.Thread(target=self._health_worker,
|
||||
name='nav-readiness', daemon=True)
|
||||
self._health_thread.start()
|
||||
|
||||
def _health_worker(self):
|
||||
while not self._closing.is_set():
|
||||
try:
|
||||
response = self._client.health()
|
||||
ready = response['ready'] and response.get('map_id') == map_id
|
||||
except Exception:
|
||||
ready = False
|
||||
with self._guard:
|
||||
self._ready, self._health_at = ready, time.monotonic()
|
||||
self._closing.wait(config.poll_interval_sec)
|
||||
|
||||
def _accept(self, request):
|
||||
try:
|
||||
# Validation uses a temporary local UUID because ROS supplies the
|
||||
# actual UUID with the accepted goal handle. Nothing is sent here.
|
||||
build_goal_body(request, '00000000-0000-4000-8000-000000000000', map_id)
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
return GoalResponse.REJECT
|
||||
with self._guard:
|
||||
if (self._reserved or self._stop_unknown or not self._ready or
|
||||
time.monotonic() - self._health_at > config.readiness_max_age_sec):
|
||||
return GoalResponse.REJECT
|
||||
self._reserved = True
|
||||
return GoalResponse.ACCEPT
|
||||
|
||||
async def _execute(self, handle):
|
||||
goal_id = str(uuid.UUID(bytes=bytes(handle.goal_id.uuid)))
|
||||
future = Future()
|
||||
try:
|
||||
session = GoalSession(self._client, build_goal_body(handle.request, goal_id, map_id))
|
||||
with self._guard:
|
||||
self._record = (handle, session, future)
|
||||
if handle.is_cancel_requested:
|
||||
session.request_cancel()
|
||||
session.start()
|
||||
except Exception as exc:
|
||||
with self._guard:
|
||||
self._stop_unknown = True
|
||||
handle.abort()
|
||||
return self._unknown_result(type(exc).__name__)
|
||||
return await future
|
||||
|
||||
def _cancel_goal(self, handle):
|
||||
with self._guard:
|
||||
record = self._record
|
||||
if record is not None and bytes(record[0].goal_id.uuid) == bytes(handle.goal_id.uuid):
|
||||
record[1].request_cancel()
|
||||
# The accepted execute callback also checks is_cancel_requested,
|
||||
# covering cancellation before a worker has been registered.
|
||||
return CancelResponse.ACCEPT
|
||||
|
||||
def _unknown_result(self, message):
|
||||
result = Navigate.Result()
|
||||
result.result.status = ExecutionResult.FAILED
|
||||
result.result.stop_state = ExecutionResult.UNKNOWN
|
||||
result.result.error_code = 'NAV_GATEWAY_STOP_UNKNOWN'
|
||||
result.result.message = message
|
||||
result.pose_valid = False
|
||||
result.errors_valid = False
|
||||
return result
|
||||
|
||||
def _pump(self):
|
||||
with self._guard:
|
||||
record = self._record
|
||||
if record is None:
|
||||
return
|
||||
handle, session, future = record
|
||||
for event in session.drain_events():
|
||||
if event.error is not None:
|
||||
with self._guard:
|
||||
self._stop_unknown = True
|
||||
self._record = None
|
||||
handle.abort()
|
||||
future.set_result(self._unknown_result(event.error))
|
||||
continue
|
||||
snapshot = event.snapshot
|
||||
if event.terminal:
|
||||
result = Navigate.Result()
|
||||
result.result.status = getattr(ExecutionResult, snapshot.outcome)
|
||||
result.result.stop_state = ExecutionResult.CONFIRMED
|
||||
result.result.message = snapshot.message
|
||||
result.result.error_code = '' if snapshot.outcome == 'COMPLETED' else 'NAV_' + snapshot.outcome
|
||||
result.result.stop_evidence_ref = f'nav-gateway:{snapshot.goal_id}:sequence:{snapshot.sequence}'
|
||||
assign_ros_time(result.result.stopped_at, snapshot.stopped_at)
|
||||
result.pose_valid = snapshot.pose_valid and snapshot.final_pose is not None
|
||||
if result.pose_valid:
|
||||
assign_ros_pose(result.final_pose, snapshot.final_pose)
|
||||
result.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
|
||||
if result.errors_valid:
|
||||
result.final_position_error = float(snapshot.position_error)
|
||||
result.final_orientation_error = float(snapshot.yaw_error)
|
||||
if snapshot.outcome == 'COMPLETED':
|
||||
handle.succeed()
|
||||
elif snapshot.outcome == 'CANCELED' and handle.is_cancel_requested:
|
||||
handle.canceled()
|
||||
else:
|
||||
if snapshot.outcome == 'CANCELED':
|
||||
# A remote cancellation has no matching ROS cancel
|
||||
# transition. Preserve physical stop, report FAILED.
|
||||
result.result.status = ExecutionResult.FAILED
|
||||
result.result.error_code = 'NAV_REMOTE_CANCELED'
|
||||
handle.abort()
|
||||
with self._guard:
|
||||
self._record = None
|
||||
self._reserved = False
|
||||
future.set_result(result)
|
||||
else:
|
||||
feedback = Navigate.Feedback()
|
||||
feedback.stamp = self.get_clock().now().to_msg()
|
||||
feedback.sequence = snapshot.sequence
|
||||
feedback.phase = (Navigate.Feedback.STOPPING if snapshot.status in
|
||||
{'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'} else
|
||||
Navigate.Feedback.CHECKING if snapshot.status == 'SENDING' else
|
||||
Navigate.Feedback.NAVIGATING)
|
||||
feedback.pose_valid = snapshot.pose_valid and snapshot.current_pose is not None
|
||||
if feedback.pose_valid:
|
||||
assign_ros_pose(feedback.current_pose, snapshot.current_pose)
|
||||
feedback.blocked_valid = False
|
||||
feedback.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
|
||||
if feedback.errors_valid:
|
||||
feedback.position_error = float(snapshot.position_error)
|
||||
feedback.orientation_error = float(snapshot.yaw_error)
|
||||
seconds = min(snapshot.elapsed, 2147483647.0)
|
||||
feedback.elapsed_time.sec = int(seconds)
|
||||
feedback.elapsed_time.nanosec = int((seconds - int(seconds)) * 1e9)
|
||||
feedback.message = snapshot.message
|
||||
handle.publish_feedback(feedback)
|
||||
|
||||
def destroy_node(self):
|
||||
self._closing.set()
|
||||
with self._guard:
|
||||
record = self._record
|
||||
if record is not None:
|
||||
record[1].request_cancel()
|
||||
self._server.destroy()
|
||||
return super().destroy_node()
|
||||
|
||||
return NavigateProxy()
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> None:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument('--endpoint', default=os.environ.get('NAV_GATEWAY_URL', 'http://127.0.0.1:8766'))
|
||||
parser.add_argument('--map-id', default=os.environ.get('NAV_GATEWAY_MAP_ID'))
|
||||
parser.add_argument('--action-name', default=os.environ.get('NAV_PROXY_ACTION_NAME'))
|
||||
for name in ('connect_timeout_sec', 'read_timeout_sec', 'request_timeout_sec',
|
||||
'poll_interval_sec', 'readiness_max_age_sec', 'feedback_silence_timeout_sec'):
|
||||
parser.add_argument('--' + name.replace('_', '-'), type=float,
|
||||
default=os.environ.get('NAV_PROXY_' + name.upper()))
|
||||
args, ros_args = parser.parse_known_args(argv)
|
||||
if not args.map_id or not args.action_name:
|
||||
parser.error('--map-id and --action-name (or matching environment variables) are required')
|
||||
try:
|
||||
config = ProxyConfig(token=os.environ.get('NAV_GATEWAY_TOKEN', ''), endpoint=args.endpoint,
|
||||
**{name: getattr(args, name) for name in ('connect_timeout_sec', 'read_timeout_sec',
|
||||
'request_timeout_sec', 'poll_interval_sec', 'readiness_max_age_sec',
|
||||
'feedback_silence_timeout_sec')})
|
||||
except ValueError as exc:
|
||||
parser.error(str(exc))
|
||||
import rclpy
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
rclpy.init(args=ros_args)
|
||||
node = create_ros_node(config, map_id=args.map_id, action_name=args.action_name)
|
||||
executor = MultiThreadedExecutor(num_threads=2)
|
||||
executor.add_node(node)
|
||||
try:
|
||||
executor.spin()
|
||||
finally:
|
||||
node.destroy_node()
|
||||
executor.shutdown()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,86 @@
|
||||
"""ROS2 lookup facade. Existing Navigate proxy still owns physical stop proof."""
|
||||
import json,threading,time,math
|
||||
from .catalog import Catalog
|
||||
|
||||
def duration_seconds(value):
|
||||
if type(value.sec) is not int or type(value.nanosec) is not int or value.sec<0 or not 0<=value.nanosec<1_000_000_000:
|
||||
raise ValueError('invalid normalized Duration')
|
||||
seconds=value.sec+value.nanosec/1e9
|
||||
if not 0<seconds<=3600:raise ValueError('Duration must be in (0,3600] seconds')
|
||||
return seconds
|
||||
|
||||
def main():
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.action import ActionServer,ActionClient,GoalResponse,CancelResponse
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from bt_skill_interfaces.action import Navigate,NavigateSemantic
|
||||
class Proxy(Node):
|
||||
def __init__(self):
|
||||
super().__init__('navigation_semantic_proxy');self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.reserved=False;self.faulted=False
|
||||
path=self.declare_parameter('site_config_file','').value
|
||||
with open(path) as f:self.catalog=Catalog(json.load(f))
|
||||
self.client=ActionClient(self,Navigate,self.declare_parameter('navigate_action','skills/navigate').value,callback_group=self.group)
|
||||
self.server=ActionServer(self,NavigateSemantic,'skills/navigate_semantic',execute_callback=self.execute,goal_callback=self.admit,cancel_callback=lambda _:CancelResponse.ACCEPT,callback_group=self.group)
|
||||
def admit(self,g):
|
||||
try:
|
||||
self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
|
||||
duration_seconds(g.timeout)
|
||||
if any(not math.isfinite(x) or x<=0 for x in (g.position_tolerance,g.orientation_tolerance)) or g.orientation_tolerance>math.pi:return GoalResponse.REJECT
|
||||
with self.lock:
|
||||
if self.reserved or self.faulted:return GoalResponse.REJECT
|
||||
self.reserved=True;self.accepted_at=time.monotonic()
|
||||
return GoalResponse.ACCEPT
|
||||
except (ValueError,KeyError):return GoalResponse.REJECT
|
||||
def execute(self,h):
|
||||
result=NavigateSemantic.Result();downstream=None;future=None
|
||||
try:
|
||||
g=h.request;lookup=self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
|
||||
deadline=self.accepted_at+duration_seconds(g.timeout)
|
||||
q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.orientation_tolerance=g.orientation_tolerance
|
||||
p=lookup['pose'];q.target_pose.header.frame_id=p['frame_id'];q.target_pose.header.stamp=self.get_clock().now().to_msg()
|
||||
for k in ('x','y','z'):setattr(q.target_pose.pose.position,k,float(p[k]))
|
||||
for k in ('x','y','z','w'):setattr(q.target_pose.pose.orientation,k,float(p['q'+k]))
|
||||
if not self.client.server_is_ready():raise RuntimeError('NAVIGATE_UNAVAILABLE')
|
||||
remaining_ns=int((deadline-time.monotonic())*1e9)
|
||||
if remaining_ns<=0 or h.is_cancel_requested:raise RuntimeError('EXPIRED_BEFORE_DISPATCH')
|
||||
q.timeout.sec=remaining_ns//1_000_000_000;q.timeout.nanosec=remaining_ns%1_000_000_000
|
||||
future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0.
|
||||
while not future.done():
|
||||
if h.is_cancel_requested or time.monotonic()>acceptance_deadline:
|
||||
def late(f):
|
||||
try:
|
||||
accepted=f.result()
|
||||
if accepted.accepted:accepted.cancel_goal_async()
|
||||
except Exception:pass
|
||||
future.add_done_callback(late);raise RuntimeError('ACCEPTANCE_UNKNOWN')
|
||||
time.sleep(.01)
|
||||
downstream=future.result()
|
||||
if not downstream.accepted:raise RuntimeError('NAVIGATE_REJECTED')
|
||||
done=downstream.get_result_async();cancel_at=None
|
||||
while not done.done():
|
||||
now=time.monotonic()
|
||||
if (h.is_cancel_requested or now>=deadline) and cancel_at is None:downstream.cancel_goal_async();cancel_at=now
|
||||
if cancel_at is not None and now-cancel_at>5:raise RuntimeError('STOP_UNKNOWN')
|
||||
if now-last_feedback>=.2:
|
||||
sequence+=1;f=NavigateSemantic.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=2 if cancel_at else 1;f.message='awaiting downstream result';h.publish_feedback(f);last_feedback=now
|
||||
time.sleep(.01)
|
||||
wrapped=done.result();m=wrapped.result
|
||||
for field in ('result','final_pose','pose_valid','final_position_error','final_orientation_error','errors_valid'):setattr(result,field,getattr(m,field))
|
||||
if wrapped.status==4 and m.result.status==0:h.succeed()
|
||||
elif wrapped.status==5 and h.is_cancel_requested:h.canceled()
|
||||
else:h.abort()
|
||||
except Exception as ex:
|
||||
self.faulted=True
|
||||
if downstream is not None and downstream.accepted:
|
||||
try:downstream.cancel_goal_async()
|
||||
except Exception:pass
|
||||
result.result.status=1;result.result.stop_state=0;result.result.error_code=str(ex);h.abort()
|
||||
finally:
|
||||
with self.lock:self.reserved=False
|
||||
return result
|
||||
rclpy.init();node=Proxy();executor=MultiThreadedExecutor(num_threads=4);executor.add_node(node)
|
||||
try:executor.spin()
|
||||
finally:executor.shutdown();node.destroy_node();rclpy.shutdown()
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,139 @@
|
||||
"""Authenticated stdlib HTTP server; ROS imports are optional and isolated."""
|
||||
import argparse
|
||||
import hmac
|
||||
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||
import ipaddress
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import threading
|
||||
import time
|
||||
from urllib.parse import urlsplit
|
||||
from .backends import MockBackend, Ros1MoveBaseBackend
|
||||
from .gateway import Gateway, GatewayError, SafetyConfig
|
||||
|
||||
|
||||
def make_server(gateway, token, host="127.0.0.1", port=8766):
|
||||
if not isinstance(token, str) or len(token) < 16: raise ValueError("bearer token must contain at least 16 characters")
|
||||
class Handler(BaseHTTPRequestHandler):
|
||||
server_version = "NavigationGateway/1"
|
||||
|
||||
def setup(self):
|
||||
super().setup()
|
||||
self.connection.settimeout(5.0) # HTTP resource bound, not a robot safety threshold.
|
||||
|
||||
def log_message(self, format, *args):
|
||||
return # Never log bearer headers or raw task input.
|
||||
|
||||
def respond(self, status, body):
|
||||
raw = json.dumps(body, allow_nan=False, separators=(",", ":")).encode()
|
||||
self.send_response(status)
|
||||
self.send_header("Content-Type", "application/json")
|
||||
self.send_header("Content-Length", str(len(raw)))
|
||||
self.send_header("Cache-Control", "no-store")
|
||||
self.end_headers()
|
||||
self.wfile.write(raw)
|
||||
|
||||
def body(self):
|
||||
if self.headers.get("Transfer-Encoding"): raise GatewayError("chunked bodies are unsupported")
|
||||
try: length = int(self.headers.get("Content-Length", "0"))
|
||||
except ValueError: raise GatewayError("invalid Content-Length")
|
||||
if not 0 < length <= 65536: raise GatewayError("body must contain 1..65536 bytes", 413)
|
||||
if self.headers.get("Content-Type", "").split(";")[0].strip() != "application/json": raise GatewayError("application/json required", 415)
|
||||
def duplicate_safe(pairs):
|
||||
obj = {}
|
||||
for key, value in pairs:
|
||||
if key in obj: raise GatewayError("duplicate JSON field")
|
||||
obj[key] = value
|
||||
return obj
|
||||
try:
|
||||
return json.loads(self.rfile.read(length), object_pairs_hook=duplicate_safe,
|
||||
parse_constant=lambda value: (_ for _ in ()).throw(GatewayError("non-finite JSON value")))
|
||||
except (ValueError, UnicodeError): raise GatewayError("invalid JSON")
|
||||
|
||||
def handle_request(self):
|
||||
try:
|
||||
expected = "Bearer " + token
|
||||
if not hmac.compare_digest(self.headers.get("Authorization", ""), expected):
|
||||
raise GatewayError("valid bearer authorization required", 401)
|
||||
parts = urlsplit(self.path)
|
||||
if parts.query or parts.fragment: raise GatewayError("query parameters unsupported")
|
||||
path = parts.path.rstrip("/")
|
||||
if self.command == "GET" and path == "/healthz": return self.respond(200, gateway.health())
|
||||
if self.command == "POST" and path == "/v1/goals": return self.respond(202, gateway.submit(self.body()))
|
||||
split = path.split("/")
|
||||
if len(split) in (4, 5) and split[1:3] == ["v1", "goals"]:
|
||||
goal_id = split[3]
|
||||
if self.command == "GET" and len(split) == 4: return self.respond(200, gateway.poll(goal_id))
|
||||
if self.command == "POST" and len(split) == 5 and split[4] == "cancel":
|
||||
if self.body() != {}: raise GatewayError("cancel body must be {}")
|
||||
return self.respond(202, gateway.cancel(goal_id))
|
||||
raise GatewayError("route not found", 404)
|
||||
except GatewayError as exc:
|
||||
self.respond(exc.http_status, {"error": str(exc)})
|
||||
except (TimeoutError, ConnectionError, BrokenPipeError):
|
||||
self.close_connection = True
|
||||
except Exception:
|
||||
self.respond(500, {"error": "internal error; query original UUID; never resubmit with a new UUID"})
|
||||
|
||||
do_GET = handle_request
|
||||
do_POST = handle_request
|
||||
|
||||
class Server(ThreadingHTTPServer):
|
||||
daemon_threads = True
|
||||
request_queue_size = 16
|
||||
return Server((host, port), Handler)
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--config", required=True, help="JSON config with explicit safety thresholds")
|
||||
parser.add_argument("--journal", required=True, help="durable SQLite journal; one per physical robot")
|
||||
parser.add_argument("--host", default="127.0.0.1")
|
||||
parser.add_argument("--port", type=int, default=8766)
|
||||
args = parser.parse_args()
|
||||
if not ipaddress.ip_address(args.host).is_loopback:
|
||||
parser.error("HTTP bearer transport listens only on loopback; use an authenticated SSH/TLS tunnel")
|
||||
token = os.environ.get("NAV_GATEWAY_TOKEN", "")
|
||||
if len(token) < 16: parser.error("set NAV_GATEWAY_TOKEN to at least 16 characters")
|
||||
config = json.loads(Path(args.config).read_text())
|
||||
safety = SafetyConfig(**config["safety"])
|
||||
if config["mode"] == "simulation":
|
||||
backend = MockBackend(map_id=config["map_id"], auto_complete_sec=config["simulation_complete_sec"])
|
||||
elif config["mode"] == "ros1_move_base":
|
||||
if config.get("production_enable") is not True: parser.error("production_enable must be explicitly true after site acceptance")
|
||||
import rospy
|
||||
rospy.init_node("navigation_http_gateway", disable_signals=True)
|
||||
backend = Ros1MoveBaseBackend(map_id=config["map_id"], **config["ros1"])
|
||||
else: parser.error("mode must be simulation or ros1_move_base")
|
||||
period = config["poll_interval_sec"]
|
||||
if isinstance(period, bool) or not isinstance(period, (int, float)) or not 0 < period <= safety.odom_max_age_sec:
|
||||
parser.error("poll_interval_sec must be positive and <= odom_max_age_sec")
|
||||
gateway = Gateway(args.journal, backend, safety)
|
||||
server = make_server(gateway, token, args.host, args.port)
|
||||
stop = threading.Event()
|
||||
def monitor():
|
||||
while not stop.wait(period):
|
||||
try: gateway.poll_all()
|
||||
except Exception:
|
||||
# Persist/telemetry errors must not release resource ownership.
|
||||
stop.set()
|
||||
threading.Thread(target=server.shutdown, daemon=True).start()
|
||||
thread = threading.Thread(target=monitor, daemon=True); thread.start()
|
||||
def shutdown(signum, frame):
|
||||
stop.set()
|
||||
threading.Thread(target=server.shutdown, daemon=True).start()
|
||||
signal.signal(signal.SIGTERM, shutdown)
|
||||
signal.signal(signal.SIGINT, shutdown)
|
||||
try: server.serve_forever(poll_interval=0.1)
|
||||
finally:
|
||||
stop.set(); thread.join(timeout=2)
|
||||
# Best effort cancel cannot turn shutdown into stop confirmation.
|
||||
for goal_id in list(gateway.records):
|
||||
try: gateway.cancel(goal_id)
|
||||
except Exception: pass
|
||||
server.server_close(); gateway.close()
|
||||
|
||||
|
||||
if __name__ == "__main__": main()
|
||||
@@ -0,0 +1,15 @@
|
||||
{
|
||||
"mode": "simulation",
|
||||
"map_id": "sim-map",
|
||||
"simulation_complete_sec": 0.5,
|
||||
"poll_interval_sec": 0.05,
|
||||
"safety": {
|
||||
"odom_max_age_sec": 0.2,
|
||||
"stationary_window_sec": 0.3,
|
||||
"linear_stopped_mps": 0.005,
|
||||
"angular_stopped_radps": 0.005,
|
||||
"pose_max_age_sec": 0.2,
|
||||
"readiness_max_age_sec": 0.5,
|
||||
"stop_wait_timeout_sec": 1.0
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user