241 lines
13 KiB
Python
241 lines
13 KiB
Python
"""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 readiness plus a timestamped blocked boolean sample; absence is unknown."""
|
|
|
|
@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, blocked=False):
|
|
self.health_value = {"ready": ready, "reason": "SIMULATION", "map_id": self.map_id,
|
|
"received_at": self.clock(), "source_fresh": True, "source_stamp": self.source_clock()}
|
|
self.health_value["blocked"] = ({"value": blocked, "received_at": self.clock(),
|
|
"source_fresh": True, "source_stamp": self.source_clock()} if type(blocked) is bool else None)
|
|
|
|
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",
|
|
"blocked": copy.deepcopy(self.health_value.get("blocked"))}
|
|
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, blocked=False):
|
|
self.sequence += 1
|
|
self.samples[goal_id] = {"controller_state": state,
|
|
"blocked": ({"value": blocked, "received_at": self.clock(), "source_fresh": source_fresh,
|
|
"source_stamp": self.source_clock()} if type(blocked) is bool else None),
|
|
"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, blocked (explicit bool); 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)
|
|
if not isinstance(data, dict): raise ValueError("health payload must be an object")
|
|
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(),
|
|
"error_code": data.get("error_code") if isinstance(data.get("error_code"), str) else ""}
|
|
value["blocked"] = ({"value": data["blocked"], **metadata, "received_at": value["received_at"]}
|
|
if type(data.get("blocked")) is bool else None)
|
|
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),
|
|
"blocked": copy.deepcopy(self.health_value.get("blocked"))}
|
|
for sample in [out["odom"], out["pose"], out["blocked"]] + out["odom_samples"]:
|
|
if isinstance(sample, dict): sample["source_fresh"] = self.source_is_fresh(sample)
|
|
self.odom_queue.clear()
|
|
return out
|