289 lines
17 KiB
Python
289 lines
17 KiB
Python
|
"""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
|