实现行为树执行器、任务协调和技能接口
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user