"""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) 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) blocked_known = self._blocked_known(health.get("blocked")) ready = health.get("ready") is True and fresh and blocked_known and not self._occupied() return {"ready": ready, "map_id": health.get("map_id"), "stamp_monotonic": self.clock(), "error_code": ("" if self._occupied() else "INPUTS_UNHEALTHY" if not blocked_known else health.get("error_code", "") if fresh else ""), "reason": "motion resource occupied or quarantined" if self._occupied() else ("blocked telemetry missing or stale" if not blocked_known 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 _blocked_known(self, sample): return (self._fresh(sample, self.config.readiness_max_age_sec) and type(sample.get("value")) is bool) 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"] or not self._blocked_known(health.get("blocked")): self.cancel(goal_id, "FAILED") if (self._fresh(health, self.config.readiness_max_age_sec) and health.get("error_code") in {"INPUTS_UNHEALTHY", "ROBOT_STATE_UNAVAILABLE", "BACKEND_NOT_CONFIGURED"}): r["error_code"] = health["error_code"] if not self._blocked_known(health.get("blocked")): r["error_code"] = "INPUTS_UNHEALTHY" 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) blocked_sample = snapshot.get("blocked") r["blocked"] = (blocked_sample["value"] if self._fresh(blocked_sample, self.config.readiness_max_age_sec) and type(blocked_sample.get("value")) is bool else None) if r["blocked"] is None: if not r["_cancel_sent"]: self.cancel(goal_id, "FAILED") r["error_code"] = "INPUTS_UNHEALTHY" 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