Fix stale error poisoning unrelated wait_command calls

parol6/client/async_client.py

@@ -1310,18 +1310,31 @@ async def wait_command(self, command_index: int, timeout: float = 10.0) -> bool:
             MotionError: If the pipeline errored at or before command_index.
         """
 
+        def _blocking_error(s: StatusBuffer) -> RobotError | None:
+            # A standing error fails this wait only when the frame proves it
+            # postdates this command's acceptance (acceptance clears any
+            # stale error server-side, and accepted_index is monotonic).
+            # Without that ordering, a frame generated before acceptance can
+            # replay a minutes-old rejection against an unrelated command.
+            # accepted_index < 0 means a pre-field status producer — keep the
+            # legacy (unordered) behavior for it.
+            err = s.error
+            if err is None or err.command_index > command_index:
+                return None
+            if s.accepted_index < 0 or s.accepted_index >= command_index:
+                return err
+            return None
+
         def _done(s: StatusBuffer) -> bool:
             if s.completed_index >= command_index:
                 return True
-            if s.error is not None and s.error.command_index <= command_index:
-                return True
-            return False
+            return _blocking_error(s) is not None
 
         ok = await self.wait_status(_done, timeout=timeout)
         if ok:
-            s = self._shared_status
-            if s.error is not None and s.error.command_index <= command_index:
-                raise MotionError(s.error)
+            err = _blocking_error(self._shared_status)
+            if err is not None:
+                raise MotionError(err)
         return ok
 
     # --------------- Move commands (queued, pre-computed trajectory) ---------------

parol6/protocol/wire.py

@@ -9,7 +9,7 @@
 Wire format uses msgpack arrays with integer type codes:
 - OK:       MsgType.OK (just the integer)
 - ERROR:    [MsgType.ERROR, message]
-- STATUS:   [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch]
+- STATUS:   [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index]
 - RESPONSE: [MsgType.RESPONSE, query_type, value]
 - COMMAND:  [CmdType.XXX, ...params]
 """
@@ -1336,6 +1336,7 @@ def pack_status(
     collision_active: bool = False,
     collision_pairs: tuple[tuple[str, str], ...] = (),
     scene_epoch: int = 0,
+    accepted_index: int = -1,
 ) -> bytes:
     """Pack a status broadcast message.
 
@@ -1378,6 +1379,7 @@ def pack_status(
             collision_active,
             collision_pairs,
             scene_epoch,
+            accepted_index,
         ),
         option=ormsgpack.OPT_SERIALIZE_NUMPY,
     )
@@ -1418,6 +1420,10 @@ class StatusBuffer:
     collision_active: bool = False
     collision_pairs: list[tuple[str, str]] = field(default_factory=list)
     scene_epoch: int = 0
+    # Last command index the server accepted; -1 from producers that predate
+    # the field. Lets waiters order a standing error against their own
+    # command's acceptance (acceptance clears stale errors server-side).
+    accepted_index: int = -1
     # Built once in __post_init__, aliasing the two enable arrays the decoder
     # mutates in place.
     cart_en: dict[str, np.ndarray] = field(init=False, repr=False, compare=False)
@@ -1462,6 +1468,7 @@ def copy(self) -> "StatusBuffer":
             collision_active=self.collision_active,
             collision_pairs=list(self.collision_pairs),
             scene_epoch=self.scene_epoch,
+            accepted_index=self.accepted_index,
         )
 
 
@@ -1473,7 +1480,8 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
                      executing_index, completed_index, last_checkpoint,
                      error, queued_segments, queued_duration, action_params,
                      tool_status_tuple, tcp_speed, simulator_active,
-                     collision_active, collision_pairs, scene_epoch]
+                     collision_active, collision_pairs, scene_epoch,
+                     accepted_index]
 
     Args:
         data: Raw msgpack bytes
@@ -1543,6 +1551,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
                     cp.append((p[0], p[1]))
         if len(msg) > 22:
             buf.scene_epoch = int(msg[22])
+        buf.accepted_index = int(msg[23]) if len(msg) > 23 else -1
 
         return True
     except Exception as e:

parol6/server/controller.py

@@ -687,6 +687,13 @@ def _handle_motion_command(
                 logger.log(TRACE, "queued_streamables_removed count=%d", removed)
             try:
                 cmd_index = self._executor.queue_command(addr, command, None)
+                # Acceptance clears the standing error, like every other
+                # command path — otherwise a rejection keeps broadcasting
+                # across minutes of streaming-only activity and poisons a
+                # later wait_command with a stale error.
+                if state.error is not None:
+                    state.error = None
+                    state.action_state = ActionState.IDLE
                 logger.log(TRACE, "Command %s queued (index=%d)", cmd_name, cmd_index)
                 if cmd_type and self._ack_policy.requires_ack(cmd_type):
                     self._reply_ok_index(addr, cmd_index)

parol6/server/status_cache.py

@@ -158,6 +158,7 @@ def __init__(self) -> None:
         # Queue tracking fields
         self._executing_index: int = -1
         self._completed_index: int = -1
+        self._accepted_index: int = -1
         self._last_checkpoint: str = ""
 
         # Error tracking field
@@ -487,11 +488,13 @@ def update_from_state(self, state: ControllerState) -> None:
         queue_changed = (
             self._executing_index != state.executing_command_index
             or self._completed_index != state.completed_command_index
+            or self._accepted_index != state.next_command_index - 1
             or self._last_checkpoint != state.last_checkpoint
         )
         if queue_changed:
             self._executing_index = state.executing_command_index
             self._completed_index = state.completed_command_index
+            self._accepted_index = state.next_command_index - 1
             self._last_checkpoint = state.last_checkpoint
 
         error_changed = self._error is not state.error
@@ -558,6 +561,7 @@ def to_binary(self) -> bytes:
                 collision_active=self._collision_active,
                 collision_pairs=self._collision_pairs,
                 scene_epoch=self._last_shapes_version,
+                accepted_index=self._accepted_index,
             )
             self._binary_dirty = False
         return self._binary_cache

tests/integration/test_stale_error_wait.py

@@ -0,0 +1,72 @@
+"""A guard-rejected command must not fail later unrelated waits.
+
+Pre-fix, ``state.error`` from a rejected move kept broadcasting (streaming
+accepts never cleared it) and ``wait_command`` honored any standing error at
+or below its index — including status frames that predate its own command's
+acceptance. The next ``wait=True`` command then raised a minutes-old
+rejection while the robot executed it fine (found live: MCP ``motion.home``
+"failing" with a stale self-collision error as the arm visibly homed).
+"""
+
+import time
+
+import numpy as np
+import pytest
+
+from parol6 import MotionError, RobotClient
+from waldoctl import Box
+
+pytestmark = pytest.mark.integration
+
+
+def _wait_until(pred, timeout: float, msg: str) -> None:
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        if pred():
+            return
+        time.sleep(0.02)
+    pytest.fail(msg)
+
+
+def _reject_move(client: RobotClient) -> None:
+    """Drive a real guard rejection: a keep-out enveloping the target wrist."""
+    import parol6.PAROL6_ROBOT as PAROL6_ROBOT
+
+    target = [0.0, -90.0, 180.0, 0.0, 0.0, 180.0]
+    p = PAROL6_ROBOT.robot.fkine(np.radians(target))[:3, 3]
+    box = Box(
+        name="blocker",
+        x=0.25,
+        y=0.25,
+        z=0.25,
+        pose=(float(p[0]), float(p[1]), float(p[2]), 0, 0, 0),
+    )
+    assert client.set_shapes([box]) == 1
+    with pytest.raises(MotionError, match="blocker"):
+        client.move_j(target, duration=1.5, wait=True)
+
+
+def test_rejected_move_does_not_poison_next_wait(client: RobotClient, server_proc):
+    """home(wait=True) right after a rejection must wait on its own outcome,
+    not raise the previous command's error off a pre-acceptance status frame."""
+    try:
+        _reject_move(client)
+        assert client.home(wait=True, timeout=30.0) >= 0
+    finally:
+        assert client.set_shapes([]) == 1
+
+
+def test_streaming_accept_clears_stale_error(client: RobotClient, server_proc):
+    """Jog acceptance clears the standing error like every other command path,
+    so a rejection can't keep broadcasting across minutes of later activity."""
+    try:
+        _reject_move(client)
+        assert client.error() is not None
+        assert client.jog_j(0, 0.2, 0.2) >= 0
+        _wait_until(
+            lambda: client.error() is None,
+            2.0,
+            "jog accept never cleared the stale error",
+        )
+    finally:
+        assert client.set_shapes([]) == 1