Merge commit 'd751bc97f5de6359d5f2fa91bf6d766304ba5bbc' into feat/execution-speed-override

# Conflicts:
#	parol6/server/controller.py

Author: Jepson2k

parol6/client/async_client.py

@@ -595,14 +595,43 @@ async def _send(self, cmd: msgspec.Struct) -> int:
         self._transport.sendto(self._tx_buf)
         return 1
 
-    async def _request(self, cmd: msgspec.Struct) -> Response | None:
+    def _drop_stale_replies(self) -> None:
+        """Discard replies already queued when a new request is about to go out.
+
+        Nothing awaits them: their caller's deadline expired mid-flight. The
+        wire carries no request id, so handing one to the next request would
+        answer it with the wrong struct and leave every later query one reply
+        behind.
+        """
+        kept = []
+        while True:
+            try:
+                item = self._rx_queue.get_nowait()
+            except asyncio.QueueEmpty:
+                break
+            try:
+                stale = isinstance(
+                    decode_message(item[0]), (ResponseMsg, OkMsg, ErrorMsg)
+                )
+            except msgspec.DecodeError:
+                stale = False
+            if not stale:
+                kept.append(item)
+        for item in kept:
+            self._rx_queue.put_nowait(item)
+
+    async def _request(
+        self, cmd: msgspec.Struct, timeout: float | None = None
+    ) -> Response | None:
         """Send a query command and wait for a typed response.
 
         Drains the receive queue until a ResponseMsg is found or timeout.
         Non-ResponseMsg datagrams (e.g. status broadcasts) are discarded.
 
         Args:
             cmd: Typed command struct
+            timeout: Per-call deadline; when given, the query is sent once
+                with no retries so the deadline is the caller's total wait.
 
         Returns:
             Typed Response struct, or None on timeout.
@@ -613,11 +642,14 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None:
         await self._ensure_endpoint()
         assert self._transport is not None
         data = encode_command(cmd)
-        for attempt in range(self.retries + 1):
+        wait = self.timeout if timeout is None else timeout
+        attempts = self.retries + 1 if timeout is None else 1
+        for attempt in range(attempts):
             try:
                 async with self._req_lock:
+                    self._drop_stale_replies()
                     self._transport.sendto(data)
-                    end_time = time.monotonic() + self.timeout
+                    end_time = time.monotonic() + wait
                     while time.monotonic() < end_time:
                         try:
                             resp_data, _ = await asyncio.wait_for(
@@ -644,7 +676,7 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None:
                 pass
             except Exception:
                 break
-            if attempt < self.retries:
+            if attempt < attempts - 1:
                 backoff = min(0.5, 0.05 * (2**attempt)) + random.uniform(0, 0.05)
                 await asyncio.sleep(backoff)
         return None
@@ -664,6 +696,7 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg:
 
         end_time = time.monotonic() + timeout
         async with self._req_lock:
+            self._drop_stale_replies()
             self._transport.sendto(data)
             while time.monotonic() < end_time:
                 try:
@@ -876,12 +909,8 @@ async def io(self, *, timeout: float | None = None) -> list[int] | None:
             isinstance(timeout, bool) or not math.isfinite(timeout) or timeout <= 0
         ):
             raise ValueError("I/O timeout must be positive and finite")
-        try:
-            async with asyncio.timeout(timeout):
-                resp = await self._request(IOCmd())
-                return resp.io if isinstance(resp, IOResultStruct) else None
-        except TimeoutError:
-            return None
+        resp = await self._request(IOCmd(), timeout=timeout)
+        return resp.io if isinstance(resp, IOResultStruct) else None
 
     async def joint_speeds(self) -> list[float] | None:
         """Current joint speeds in steps/sec [J1, J2, J3, J4, J5, J6].

parol6/server/controller.py

@@ -345,13 +345,7 @@ def _handle_estop(self, state: ControllerState) -> None:
                 logger.warning("E-STOP activated")
                 self.estop_active = True
                 self._segment_player.cancel(state)
-                self._planner.sync_tool(
-                    state.current_tool,
-                    variant_key=state.current_tool_variant,
-                    tcp_offset_m=state.tcp_offset_m,
-                    tcp_rotation_rad=state.tcp_rotation_rad,
-                )
-                self._planner.sync_shapes(state.shapes)
+                self._resync_planner(state)
                 if self._executor.active_command:
                     self._executor.cancel_active_command("E-Stop activated")
                 self._executor.clear_queue("E-Stop activated")
@@ -812,6 +806,22 @@ def _handle_query(
                 addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=str(e))
             )
 
+    def _resync_planner(self, state: ControllerState) -> None:
+        """Bring the planner subprocess back to the controller's tool and world.
+
+        The planner applies SET_TCP_TRANSFORM / SELECT_TOOL / SET_SHAPES at
+        plan time, when the command is still queued. Cancelling that queue
+        leaves the planner holding a change the controller never applied,
+        and every later plan would be solved against it.
+        """
+        self._planner.sync_tool(
+            state.current_tool,
+            variant_key=state.current_tool_variant,
+            tcp_offset_m=state.tcp_offset_m,
+            tcp_rotation_rad=state.tcp_rotation_rad,
+        )
+        self._planner.sync_shapes(state.shapes)
+
     def _handle_system_command(
         self,
         command: SystemCommand,
@@ -842,6 +852,7 @@ def _handle_system_command(
                 self._segment_player.cancel(state)
                 self._executor.cancel_active_command(reason)
                 self._executor.clear_queue(reason)
+                self._resync_planner(state)
                 # A pause holds the queue it interrupted; that queue is gone.
                 state.execution_paused = False
 
@@ -852,14 +863,8 @@ def _handle_system_command(
                 self._segment_player.cancel(state)
                 self._executor.cancel_active_command("Reset")
                 self._executor.clear_queue("Reset")
+                self._resync_planner(state)
                 state.execution_paused = False
-                self._planner.sync_tool(
-                    state.current_tool,
-                    variant_key=state.current_tool_variant,
-                    tcp_offset_m=state.tcp_offset_m,
-                    tcp_rotation_rad=state.tcp_rotation_rad,
-                )
-                self._planner.sync_shapes(state.shapes)
 
             # Infrastructure side effects (only 2-3 commands trigger these)
             if command._switch_simulator is not None:

tests/integration/test_digital_io.py

@@ -48,3 +48,32 @@ async def query(rbt):
                         await absent.write_io(0, 1, timeout=invalid)
 
     asyncio.run(missing_peer())
+
+
+def test_late_replies_never_answer_the_next_request(ports, server_proc):
+    """A reply that lands after its caller's deadline expired is not served
+    to the next request: the wire carries no request ids, so a stale reply
+    would answer the wrong query and leave every later one a reply behind."""
+    from parol6.protocol.wire import IOResultStruct, pack_ok, pack_response
+
+    async def scenario():
+        async with AsyncRobotClient(
+            host=ports.server_ip, port=ports.server_port, timeout=5.0
+        ) as rbt:
+            await rbt._ensure_endpoint()
+            peer = (ports.server_ip, ports.server_port)
+            rbt._rx_queue.put_nowait(
+                (pack_response(IOResultStruct(io=[0, 0, 0, 0, 1])), peer)
+            )
+            pose = await rbt.pose()
+            assert pose is not None and len(pose) == 6
+            assert await rbt.angles() is not None
+            rbt._rx_queue.put_nowait((pack_ok(), peer))
+            index = await rbt.delay(0.1)
+            assert index >= 1, "a stale index-less OK must not stand in for the ack"
+            assert await rbt.wait_command(index, timeout=5)
+            for _ in range(5):
+                await rbt.io(timeout=1e-4)
+            assert await rbt.pose() is not None
+
+    asyncio.run(scenario())

tests/integration/test_stop_semantics.py

@@ -89,3 +89,31 @@ def test_estop_latches_until_reset(client: RobotClient, server_proc):
         "canceled motion resurfaced after reset"
     )
     assert client.home(wait=True, timeout=30.0) >= 0
+
+
+def test_stop_discards_a_queued_tcp_transform_from_the_planner_too(
+    client: RobotClient, server_proc
+):
+    """The planner applies SET_TCP_TRANSFORM when it plans, while the command
+    is still queued; a Stop that cancels the queue must take that transform
+    back from the planner, or every later plan is solved against a TCP the
+    controller never applied."""
+    assert client.home(wait=True, timeout=30.0) >= 0
+    index = client.set_tcp_transform(0, 0, 0, 0, 0, 0)
+    assert index >= 0 and client.wait_command(index, timeout=5.0)
+    start = client.angles()
+    pose = client.pose()
+    assert start is not None and pose is not None
+    assert client.delay(3.0) >= 0
+    assert client.set_tcp_transform(0, 0, 40, 0, 90, 0) >= 0
+    assert client.stop() == 1
+    assert client.tcp_transform() == pytest.approx([0] * 6)
+    # A move to the pose the arm is already at is planned against the
+    # controller's TCP and therefore goes nowhere.
+    index = client.move_l(pose, duration=1.0, wait=False)
+    assert index >= 0 and client.wait_command(index, timeout=10.0)
+    after = client.angles()
+    assert after is not None
+    assert np.allclose(after, start, atol=0.5), (
+        f"the planner kept the cancelled TCP: {start} -> {after}"
+    )