behavior-tree/navigation_gateway/backends.py

"""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