fix: align navigation contract and readiness handling

2026-09-22 14:35:14 +08:00
parent 91bcd92d6b
commit 964d1fde67
21 changed files with 922 additions and 139 deletions
@@ -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
@@ -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:
@@ -21,7 +21,7 @@ from urllib.parse import urlsplit


 _STATES = {'SENDING', 'ACTIVE', 'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'}
-_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED'}
+_OUTCOMES = {'COMPLETED', 'FAILED', 'CANCELED', 'TIMED_OUT', 'REJECTED', 'BLOCKED', 'NOT_READY'}
 _CONTROLLER_TERMINALS = {'ARRIVED', 'SUCCEEDED', 'ABORTED', 'PREEMPTED', 'RECALLED', 'REJECTED'}
 _TRACE_FIELDS = ('task_id', 'subtask_id', 'attempt', 'task_revision', 'plan_version',
                 'run_id', 'execution_generation')
@@ -92,18 +92,24 @@ class GatewaySnapshot:
    current_pose: dict[str, Any] | None = None
    final_pose: dict[str, Any] | None = None
    stopped_at: float | None = None
+    blocked: bool | None = None
+    error_code: str = ''

    @classmethod
    def parse(cls, data: Mapping[str, Any], goal_id: str) -> 'GatewaySnapshot':
        try:
            required = {name: data[name] for name in cls.__dataclass_fields__
-                        if name not in {'pose_valid', 'current_pose', 'final_pose', 'stopped_at'}}
+                        if name not in {'pose_valid', 'current_pose', 'final_pose', 'stopped_at', 'blocked', 'error_code'}}
            snapshot = cls(**required, pose_valid=data.get('pose_valid', False),
                current_pose=validate_observed_pose(data.get('current_pose')),
                final_pose=validate_observed_pose(data.get('final_pose')),
-                stopped_at=data.get('stopped_at'))
+                stopped_at=data.get('stopped_at'), blocked=data.get('blocked'), error_code=data.get('error_code', ''))
        except (TypeError, KeyError) as exc:
            raise GatewayError('Gateway snapshot is missing required fields') from exc
+        if snapshot.blocked is not None and type(snapshot.blocked) is not bool:
+            raise GatewayError('Gateway blocked must be boolean or unknown')
+        if not isinstance(snapshot.error_code, str):
+            raise GatewayError('Gateway error_code must be text')
        stop_time_parts = None if snapshot.stopped_at is None else _ros_time_parts(snapshot.stopped_at)
        if type(snapshot.pose_valid) is not bool:
            raise GatewayError('Gateway pose_valid must be boolean')
@@ -191,6 +197,37 @@ def assign_ros_pose(destination: Any, source: Mapping[str, Any]) -> None:
            setattr(getattr(destination.pose, name), axis, float(source[name][axis]))


+def navigation_status(snapshot, enum):
+    """Translate wire outcomes by name; execution and navigation numbers differ."""
+    names = {'COMPLETED': 'SUCCEEDED', 'CANCELED': 'CANCELED', 'TIMED_OUT': 'TIMEOUT',
+             'BLOCKED': 'BLOCKED', 'NOT_READY': 'NOT_READY', 'REJECTED': 'FAILED', 'FAILED': 'FAILED'}
+    if snapshot.outcome in {'FAILED', 'REJECTED', 'NOT_READY'} and snapshot.error_code in {'INPUTS_UNHEALTHY', 'ROBOT_STATE_UNAVAILABLE', 'BACKEND_NOT_CONFIGURED'}:
+        return enum.NOT_READY
+    return getattr(enum, names[snapshot.outcome])
+
+
+def assign_navigation_feedback(feedback, snapshot):
+    """A required bool cannot truthfully encode missing blocked telemetry."""
+    if snapshot.blocked is None:
+        raise GatewayError('blocked telemetry unavailable; stop requested')
+    feedback.sequence = snapshot.sequence
+    feedback.phase = (feedback.STOPPING if snapshot.status in {'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'}
+        else feedback.CHECKING if snapshot.status == 'SENDING'
+        else feedback.BLOCKED if snapshot.blocked else feedback.NAVIGATING)
+    feedback.blocked = snapshot.blocked
+    feedback.current_pose_valid = snapshot.pose_valid and snapshot.current_pose is not None
+    if feedback.current_pose_valid:
+        assign_ros_pose(feedback.current_pose, snapshot.current_pose)
+    feedback.error_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
+    if feedback.error_valid:
+        feedback.position_error = float(snapshot.position_error)
+        feedback.yaw_error = float(snapshot.yaw_error)
+    seconds = min(snapshot.elapsed, 2147483647.0)
+    feedback.elapsed_time.sec = int(seconds)
+    feedback.elapsed_time.nanosec = int((seconds - int(seconds)) * 1e9)
+    feedback.message = snapshot.message
+
+
 class GatewayClient:
    """One fresh bounded connection per request; no redirect or proxy handling."""

@@ -396,14 +433,27 @@ def build_goal_body(goal: Any, goal_id: str, map_id: str) -> dict[str, Any]:
            'orientation': {axis: getattr(pose.orientation, axis) for axis in ('x', 'y', 'z', 'w')},
        },
        'position_tolerance': _positive(goal.position_tolerance, 'position_tolerance'),
-        'yaw_tolerance': _positive(goal.orientation_tolerance, 'orientation_tolerance'),
+        'yaw_tolerance': _positive(goal.yaw_tolerance, 'yaw_tolerance'),
        'timeout_sec': _positive(timeout, 'timeout'),
    }
-    if not body['trace']['task_id'] or not body['trace']['subtask_id'] or body['trace']['attempt'] < 1:
-        raise ValueError('TaskTrace task_id, subtask_id and attempt are required')
+    for name in ('task_id', 'subtask_id', 'run_id'):
+        if not isinstance(body['trace'][name], str) or not body['trace'][name]:
+            raise ValueError('TaskTrace identifiers must be nonempty strings')
+    for name in ('attempt', 'task_revision', 'plan_version', 'execution_generation'):
+        value = body['trace'][name]
+        maximum = 2**64 if name == 'execution_generation' else 2**32
+        if type(value) is not int or not 0 < value < maximum:
+            raise ValueError('TaskTrace counters must be positive and fit their wire types')
    values = list(body['target_pose']['position'].values()) + list(body['target_pose']['orientation'].values())
    if any(isinstance(v, bool) or not isinstance(v, (float, int)) or not math.isfinite(v) for v in values):
        raise ValueError('target pose must contain finite coordinates')
+    orientation = body['target_pose']['orientation']
+    scale = max(abs(v) for v in orientation.values())
+    if scale == 0:
+        raise ValueError('target quaternion must be nonzero')
+    scaled = {axis: value / scale for axis, value in orientation.items()}
+    norm = math.sqrt(sum(v*v for v in scaled.values()))
+    body['target_pose']['orientation'] = {axis: value / norm for axis, value in scaled.items()}
    return json.loads(json.dumps(body, allow_nan=False))


@@ -418,7 +468,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
    from rclpy.node import Node
    from rclpy.task import Future
    from bt_skill_interfaces.action import Navigate
-    from bt_skill_interfaces.msg import ExecutionResult
+    from bt_skill_interfaces.msg import NavigationResult

    if not map_id or not action_name:
        raise ValueError('map_id and action_name must be explicitly configured')
@@ -430,6 +480,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
            self._guard = threading.Lock()
            self._ready = False
            self._health_at = 0.0
+            self._health_error_code = ''
            self._reserved = False
            self._stop_unknown = False
            self._record = None
@@ -448,10 +499,14 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
                try:
                    response = self._client.health()
                    ready = response['ready'] and response.get('map_id') == map_id
+                    error_code = response.get('error_code', '')
+                    if not isinstance(error_code, str): error_code = ''
                except Exception:
                    ready = False
+                    error_code = ''
                with self._guard:
                    self._ready, self._health_at = ready, time.monotonic()
+                    self._health_error_code = error_code
                self._closing.wait(config.poll_interval_sec)

        def _accept(self, request):
@@ -462,13 +517,23 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
            except (AttributeError, TypeError, ValueError):
                return GoalResponse.REJECT
            with self._guard:
-                if (self._reserved or self._stop_unknown or not self._ready or
-                        time.monotonic() - self._health_at > config.readiness_max_age_sec):
+                if self._reserved or self._stop_unknown:
                    return GoalResponse.REJECT
                self._reserved = True
            return GoalResponse.ACCEPT

        async def _execute(self, handle):
+            with self._guard:
+                fresh = time.monotonic() - self._health_at <= config.readiness_max_age_sec
+                ready = self._ready and fresh
+                if not ready:
+                    code = self._health_error_code if fresh else ''
+                    self._reserved = False
+            if not ready:
+                result = self._unknown_result('navigation backend is not ready; no goal dispatched', code or 'NAV_NOT_READY')
+                result.result.status = NavigationResult.NOT_READY
+                handle.abort()
+                return result
            goal_id = str(uuid.UUID(bytes=bytes(handle.goal_id.uuid)))
            future = Future()
            try:
@@ -494,14 +559,16 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
            # covering cancellation before a worker has been registered.
            return CancelResponse.ACCEPT

-        def _unknown_result(self, message):
+        def _unknown_result(self, message, error_code='NAV_GATEWAY_STOP_UNKNOWN'):
            result = Navigate.Result()
-            result.result.status = ExecutionResult.FAILED
-            result.result.stop_state = ExecutionResult.UNKNOWN
-            result.result.error_code = 'NAV_GATEWAY_STOP_UNKNOWN'
+            result.result.status = (NavigationResult.NOT_READY if error_code in
+                {'INPUTS_UNHEALTHY', 'ROBOT_STATE_UNAVAILABLE', 'BACKEND_NOT_CONFIGURED'} else NavigationResult.FAILED)
+            result.result.stop_state = NavigationResult.UNKNOWN
+            result.result.error_code = error_code
            result.result.message = message
-            result.pose_valid = False
-            result.errors_valid = False
+            result.final_pose_valid = False
+            result.final_position_error = math.nan
+            result.final_yaw_error = math.nan
            return result

        def _pump(self):
@@ -521,19 +588,17 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
                snapshot = event.snapshot
                if event.terminal:
                    result = Navigate.Result()
-                    result.result.status = getattr(ExecutionResult, snapshot.outcome)
-                    result.result.stop_state = ExecutionResult.CONFIRMED
+                    result.result.status = navigation_status(snapshot, NavigationResult)
+                    result.result.stop_state = NavigationResult.CONFIRMED
                    result.result.message = snapshot.message
-                    result.result.error_code = '' if snapshot.outcome == 'COMPLETED' else 'NAV_' + snapshot.outcome
+                    result.result.error_code = snapshot.error_code or ('' if snapshot.outcome == 'COMPLETED' else 'NAV_' + snapshot.outcome)
                    result.result.stop_evidence_ref = f'nav-gateway:{snapshot.goal_id}:sequence:{snapshot.sequence}'
                    assign_ros_time(result.result.stopped_at, snapshot.stopped_at)
-                    result.pose_valid = snapshot.pose_valid and snapshot.final_pose is not None
-                    if result.pose_valid:
+                    result.final_pose_valid = snapshot.pose_valid and snapshot.final_pose is not None
+                    if result.final_pose_valid:
                        assign_ros_pose(result.final_pose, snapshot.final_pose)
-                    result.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
-                    if result.errors_valid:
-                        result.final_position_error = float(snapshot.position_error)
-                        result.final_orientation_error = float(snapshot.yaw_error)
+                    result.final_position_error = math.nan if snapshot.position_error is None else float(snapshot.position_error)
+                    result.final_yaw_error = math.nan if snapshot.yaw_error is None else float(snapshot.yaw_error)
                    if snapshot.outcome == 'COMPLETED':
                        handle.succeed()
                    elif snapshot.outcome == 'CANCELED' and handle.is_cancel_requested:
@@ -542,7 +607,7 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
                        if snapshot.outcome == 'CANCELED':
                            # A remote cancellation has no matching ROS cancel
                            # transition. Preserve physical stop, report FAILED.
-                            result.result.status = ExecutionResult.FAILED
+                            result.result.status = NavigationResult.FAILED
                            result.result.error_code = 'NAV_REMOTE_CANCELED'
                        handle.abort()
                    with self._guard:
@@ -552,23 +617,16 @@ def create_ros_node(config: ProxyConfig, *, map_id: str, action_name: str):
                else:
                    feedback = Navigate.Feedback()
                    feedback.stamp = self.get_clock().now().to_msg()
-                    feedback.sequence = snapshot.sequence
-                    feedback.phase = (Navigate.Feedback.STOPPING if snapshot.status in
-                        {'CANCEL_REQUESTED', 'STOP_UNKNOWN', 'TERMINAL'} else
-                        Navigate.Feedback.CHECKING if snapshot.status == 'SENDING' else
-                        Navigate.Feedback.NAVIGATING)
-                    feedback.pose_valid = snapshot.pose_valid and snapshot.current_pose is not None
-                    if feedback.pose_valid:
-                        assign_ros_pose(feedback.current_pose, snapshot.current_pose)
-                    feedback.blocked_valid = False
-                    feedback.errors_valid = snapshot.position_error is not None and snapshot.yaw_error is not None
-                    if feedback.errors_valid:
-                        feedback.position_error = float(snapshot.position_error)
-                        feedback.orientation_error = float(snapshot.yaw_error)
-                    seconds = min(snapshot.elapsed, 2147483647.0)
-                    feedback.elapsed_time.sec = int(seconds)
-                    feedback.elapsed_time.nanosec = int((seconds - int(seconds)) * 1e9)
-                    feedback.message = snapshot.message
+                    try:
+                        assign_navigation_feedback(feedback, snapshot)
+                    except GatewayError as exc:
+                        session.request_cancel()
+                        with self._guard:
+                            self._stop_unknown = True
+                            self._record = None
+                        handle.abort()
+                        future.set_result(self._unknown_result(str(exc), snapshot.error_code or 'NAV_GATEWAY_STOP_UNKNOWN'))
+                        continue
                    handle.publish_feedback(feedback)

        def destroy_node(self):
@@ -9,6 +9,31 @@ def duration_seconds(value):
    if not 0<seconds<=3600:raise ValueError('Duration must be in (0,3600] seconds')
    return seconds

+def translate_navigation_result(source, target, navigation_enum, execution_enum):
+    """Preserve proof and diagnostics while explicitly translating different enums."""
+    mapping = {navigation_enum.SUCCEEDED: execution_enum.COMPLETED,
+               navigation_enum.CANCELED: execution_enum.CANCELED,
+               navigation_enum.TIMEOUT: execution_enum.TIMED_OUT,
+               navigation_enum.BLOCKED: execution_enum.FAILED,
+               navigation_enum.NOT_READY: execution_enum.REJECTED,
+               navigation_enum.FAILED: execution_enum.FAILED}
+    target.result.status = mapping[source.result.status]
+    target.result.stop_state = {navigation_enum.UNKNOWN: execution_enum.UNKNOWN,
+                               navigation_enum.CONFIRMED: execution_enum.CONFIRMED}[source.result.stop_state]
+    for field in ('error_code', 'message', 'stopped_at', 'stop_evidence_ref'):
+        setattr(target.result, field, getattr(source.result, field))
+    if not target.result.error_code:
+        target.result.error_code = {navigation_enum.BLOCKED: 'NAV_BLOCKED',
+                                    navigation_enum.NOT_READY: 'NAV_NOT_READY'}.get(source.result.status, '')
+    target.final_pose = source.final_pose
+    target.pose_valid = source.final_pose_valid
+    target.errors_valid = (source.final_pose_valid and
+        all(math.isfinite(v) for v in (source.final_position_error, source.final_yaw_error)) and
+        source.final_position_error >= 0 and abs(source.final_yaw_error) <= math.pi)
+    target.final_position_error = source.final_position_error
+    target.final_orientation_error = source.final_yaw_error
+
+
 def main():
    import rclpy
    from rclpy.node import Node
@@ -16,6 +41,8 @@ def main():
    from rclpy.callback_groups import ReentrantCallbackGroup
    from rclpy.executors import MultiThreadedExecutor
    from bt_skill_interfaces.action import Navigate,NavigateSemantic
+    from bt_skill_interfaces.msg import NavigationResult,ExecutionResult
+    from action_msgs.msg import GoalStatus
    class Proxy(Node):
        def __init__(self):
            super().__init__('navigation_semantic_proxy');self.group=ReentrantCallbackGroup();self.lock=threading.Lock();self.reserved=False;self.faulted=False
@@ -38,7 +65,7 @@ def main():
            try:
                g=h.request;lookup=self.catalog.resolve(g.kind,g.reference,g.registry_version,shelf=g.shelf_id,side=g.side_id,column=g.column_id,tier=g.tier_id)
                deadline=self.accepted_at+duration_seconds(g.timeout)
-                q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.orientation_tolerance=g.orientation_tolerance
+                q=Navigate.Goal();q.trace=g.trace;q.position_tolerance=g.position_tolerance;q.yaw_tolerance=g.orientation_tolerance
                p=lookup['pose'];q.target_pose.header.frame_id=p['frame_id'];q.target_pose.header.stamp=self.get_clock().now().to_msg()
                for k in ('x','y','z'):setattr(q.target_pose.pose.position,k,float(p[k]))
                for k in ('x','y','z','w'):setattr(q.target_pose.pose.orientation,k,float(p['q'+k]))
@@ -46,7 +73,12 @@ def main():
                remaining_ns=int((deadline-time.monotonic())*1e9)
                if remaining_ns<=0 or h.is_cancel_requested:raise RuntimeError('EXPIRED_BEFORE_DISPATCH')
                q.timeout.sec=remaining_ns//1_000_000_000;q.timeout.nanosec=remaining_ns%1_000_000_000
-                future=self.client.send_goal_async(q);acceptance_deadline=min(deadline,time.monotonic()+3.);sequence=0;last_feedback=0.
+                def forward_feedback(event):
+                    source=event.feedback
+                    f=NavigateSemantic.Feedback()
+                    for field in ('stamp','sequence','phase','message'):setattr(f,field,getattr(source,field))
+                    h.publish_feedback(f)
+                future=self.client.send_goal_async(q,feedback_callback=forward_feedback);acceptance_deadline=min(deadline,time.monotonic()+3.)
                while not future.done():
                    if h.is_cancel_requested or time.monotonic()>acceptance_deadline:
                        def late(f):
@@ -63,20 +95,18 @@ def main():
                    now=time.monotonic()
                    if (h.is_cancel_requested or now>=deadline) and cancel_at is None:downstream.cancel_goal_async();cancel_at=now
                    if cancel_at is not None and now-cancel_at>5:raise RuntimeError('STOP_UNKNOWN')
-                    if now-last_feedback>=.2:
-                        sequence+=1;f=NavigateSemantic.Feedback();f.stamp=self.get_clock().now().to_msg();f.sequence=sequence;f.phase=2 if cancel_at else 1;f.message='awaiting downstream result';h.publish_feedback(f);last_feedback=now
                    time.sleep(.01)
                wrapped=done.result();m=wrapped.result
-                for field in ('result','final_pose','pose_valid','final_position_error','final_orientation_error','errors_valid'):setattr(result,field,getattr(m,field))
-                if wrapped.status==4 and m.result.status==0:h.succeed()
-                elif wrapped.status==5 and h.is_cancel_requested:h.canceled()
+                translate_navigation_result(m,result,NavigationResult,ExecutionResult)
+                if wrapped.status==GoalStatus.STATUS_SUCCEEDED and m.result.status==NavigationResult.SUCCEEDED:h.succeed()
+                elif wrapped.status==GoalStatus.STATUS_CANCELED and m.result.status==NavigationResult.CANCELED and h.is_cancel_requested:h.canceled()
                else:h.abort()
            except Exception as ex:
                self.faulted=True
                if downstream is not None and downstream.accepted:
                    try:downstream.cancel_goal_async()
                    except Exception:pass
-                result.result.status=1;result.result.stop_state=0;result.result.error_code=str(ex);h.abort()
+                result.result.status=ExecutionResult.FAILED;result.result.stop_state=ExecutionResult.UNKNOWN;result.result.error_code=str(ex);h.abort()
            finally:
                with self.lock:self.reserved=False
            return result