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/gateway.py
+22 -4
View File
@@ -55,7 +55,6 @@ def validate_goal(body):
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)
if body["yaw_tolerance"] > math.pi: raise GatewayError("yaw_tolerance is radians and must be <= pi")
return copy.deepcopy(body)
@@ -130,10 +129,14 @@ class Gateway:
try: health = self.backend.health()
except Exception: health = {}
fresh = self._fresh(health, self.config.readiness_max_age_sec)
ready = health.get("ready") is True and fresh and not self._occupied()
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
(health.get("reason", "") if fresh else "readiness missing or stale")}
("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)
@@ -189,6 +192,10 @@ class Gateway:
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)
@@ -198,14 +205,25 @@ class Gateway:
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"]:
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: