wangfeiyu/behavior-tree
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: align navigation contract and readiness handling

Browse Source
This commit is contained in:
wangfeiyu
2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation_gateway/backends.py
+18 -8
View File
@@ -16,7 +16,7 @@ class NavigationBackend(ABC):
@abstractmethod
def health(self):
"""Return ready, reason, map_id, received_at (local monotonic), source_fresh."""
"""Return readiness plus a timestamped blocked boolean sample; absence is unknown."""
@abstractmethod
def send(self, goal):
@@ -48,9 +48,11 @@ class MockBackend(NavigationBackend):
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):
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)
@@ -59,7 +61,8 @@ class MockBackend(NavigationBackend):
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"}
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):
@@ -67,9 +70,11 @@ class MockBackend(NavigationBackend):
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):
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,
@@ -92,7 +97,7 @@ class Ros1MoveBaseBackend(NavigationBackend):
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.
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):
@@ -176,10 +181,14 @@ class Ros1MoveBaseBackend(NavigationBackend):
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()}
"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()}
@@ -223,8 +232,9 @@ class Ros1MoveBaseBackend(NavigationBackend):
# 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"]:
"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