From 2a0bb219b44bb677d8249a46093e6e31d41c4407 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 14 Jul 2026 12:55:58 -0400 Subject: [PATCH] Fix stale error poisoning unrelated wait_command calls MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A guard rejection's state.error kept broadcasting across streaming-only activity (jog accepts never cleared it), and wait_command honored any standing error at or below its index — including frames that predate its own command's acceptance. The next wait=True command then raised a minutes-old rejection while the robot executed fine (seen live as a home->home move "failing" with a stale self-collision error). Streaming accepts now clear the standing error like every other accept path, and STATUS carries accepted_index (the last accepted command index, len-guarded for old producers) so wait_command only honors an error once the frame proves it postdates the waiter's acceptance. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01XbP8n8tzaJXKMjkRxGnoa1 --- parol6/client/async_client.py | 25 ++++++-- parol6/protocol/wire.py | 13 +++- parol6/server/controller.py | 7 +++ parol6/server/status_cache.py | 4 ++ tests/integration/test_stale_error_wait.py | 72 ++++++++++++++++++++++ 5 files changed, 113 insertions(+), 8 deletions(-) create mode 100644 tests/integration/test_stale_error_wait.py diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index abf1300..6f5eaae 100644 --- a/parol6/client/async_client.py +++ b/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) --------------- diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 4f4307d..cebcf54 100644 --- a/parol6/protocol/wire.py +++ b/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: diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 7808e2d..7d9b0ab 100644 --- a/parol6/server/controller.py +++ b/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) diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 767c7b1..10ffd5c 100644 --- a/parol6/server/status_cache.py +++ b/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 diff --git a/tests/integration/test_stale_error_wait.py b/tests/integration/test_stale_error_wait.py new file mode 100644 index 0000000..fa34301 --- /dev/null +++ b/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