From 7e48e6f71f561620c6db8dc8b0106309f40ac1ad Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Mon, 7 Sep 2026 12:19:26 -0400 Subject: [PATCH 1/5] Bound digital I/O requests and support named signal skills --- README.md | 10 ++++++ parol6/client/async_client.py | 41 +++++++++++++++++++---- parol6/client/dry_run_client.py | 20 ++++++++++- parol6/client/sync_client.py | 8 ++--- pyproject.toml | 2 +- tests/integration/test_digital_io.py | 50 ++++++++++++++++++++++++++++ 6 files changed, 118 insertions(+), 13 deletions(-) create mode 100644 tests/integration/test_digital_io.py diff --git a/README.md b/README.md index 673fbdd..c28f699 100644 --- a/README.md +++ b/README.md @@ -451,3 +451,13 @@ The existing `set_tcp_offset(x, y, z)` clears user rotation and now returns its queued index for confirmation. `tcp_offset()` still reads three translations; `tcp_transform()` reads all six values. Both raise `TimeoutError` when no valid reply arrives instead of reporting a misleading zero correction. + +Digital I/O reads and writes accept an optional per-call `timeout` in seconds: +`rbt.io(timeout=1.0)` returns `None` without a reply, while +`rbt.write_io(0, 1, timeout=1.0)` raises `TimeoutError` if acceptance remains +unconfirmed. The deadline includes transport setup and retries. Omitting it +retains the configured client timeout. The same options work on the sync client. +The client advertises `io.digital` for typed named-signal skills, which can be +imported from `waldo_commander.skills`; mappings are `waldoctl.signals.DigitalSignal` +values stored in a setup snapshot. Dry-run clients advertise `execution.preview` +so those skills require explicit observation fixtures during preview. diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 4d6f723..b159124 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -5,6 +5,7 @@ import asyncio import contextlib import logging +import math import random import socket import struct @@ -257,7 +258,11 @@ class AsyncRobotClient(_RobotClientABC): @property def skill_capabilities(self) -> frozenset[str]: - return super().skill_capabilities | {"backend.parol6", "tool.gripper"} + return super().skill_capabilities | { + "backend.parol6", + "tool.gripper", + "io.digital", + } def __init__( self, @@ -850,16 +855,26 @@ async def angles(self) -> list[float] | None: resp = await self._request(AnglesCmd()) return resp.angles if isinstance(resp, AnglesResultStruct) else None - async def io(self) -> list[int] | None: + async def io(self, *, timeout: float | None = None) -> list[int] | None: """Digital I/O status [in1, in2, out1, out2, estop]. + ``timeout`` bounds setup, retries, and the reply; None uses client defaults. + Category: Query Example: io = rbt.io() """ - resp = await self._request(IOCmd()) - return resp.io if isinstance(resp, IOResultStruct) else None + if timeout is not None and ( + 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 async def joint_speeds(self) -> list[float] | None: """Current joint speeds in steps/sec [J1, J2, J3, J4, J5, J6]. @@ -1847,7 +1862,9 @@ async def jog_l( # --------------- IO / Gripper / Utility --------------- - async def write_io(self, index: int, value: int) -> int: + async def write_io( + self, index: int, value: int, *, timeout: float | None = None + ) -> int: """Set digital output by logical index (0 = first output pin). The firmware I/O byte layout is ``[in0, in1, out0, out1, estop, ...]`` @@ -1855,6 +1872,9 @@ async def write_io(self, index: int, value: int) -> int: Returns the command index (≥ 0) on success, -1 on failure. + ``timeout`` bounds command acceptance. TimeoutError leaves application + unconfirmed; None uses the client defaults. + Category: I/O Example: @@ -1866,8 +1886,15 @@ async def write_io(self, index: int, value: int) -> int: raise ValueError("I/O value must be 0 or 1") # Firmware bit layout: [in0, in1, out0, out1, estop, ...] firmware_index = index + 2 - result = await self._send(WriteIOCmd(port_index=firmware_index, value=value)) - return result + if timeout is not None and ( + isinstance(timeout, bool) or not math.isfinite(timeout) or timeout <= 0 + ): + raise ValueError("I/O timeout must be positive and finite") + async with asyncio.timeout(timeout): + result = await self._send( + WriteIOCmd(port_index=firmware_index, value=value) + ) + return result async def delay(self, seconds: float) -> int: """Insert a non-blocking delay in the motion queue. diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 7890425..9387727 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -43,6 +43,7 @@ SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd, + WriteIOCmd, TeleportCmd, ToolActionCmd, ) @@ -546,7 +547,14 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: @property def skill_capabilities(self) -> frozenset[str]: return frozenset( - {"motion.joint", "motion.linear", "tool.gripper", "backend.parol6"} + { + "motion.joint", + "motion.linear", + "tool.gripper", + "backend.parol6", + "io.digital", + "execution.preview", + } ) def angles(self) -> list[float]: @@ -601,6 +609,16 @@ def servo_j( return self._dispatch(build_cmd("servo_j_pose", pose, **kwargs)) return self._dispatch(build_cmd("servo_j", angles or [], **kwargs)) + def write_io(self, index: int, value: int, *, timeout: float | None = None) -> int: + if type(index) is not int or index not in (0, 1): + raise ValueError("Output index must be 0 or 1") + if type(value) not in (int, bool) or value not in (0, 1): + raise ValueError("Digital output must be 0 or 1") + result = self._dispatch(WriteIOCmd(port_index=index + 2, value=int(value))) + if result is not None and result.error is not None: + raise RuntimeError(str(result.error)) + return 0 + def delay(self, seconds: float = 0.0) -> None: pass diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 1cc1054..b1d3598 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -287,13 +287,13 @@ def angles(self) -> list[float] | None: """ return _run(self._inner.angles()) - def io(self) -> list[int] | None: + def io(self, *, timeout: float | None = None) -> list[int] | None: """Digital I/O status. Returns: List of 5 integers [in1, in2, out1, out2, estop], or None on timeout. """ - return _run(self._inner.io()) + return _run(self._inner.io(timeout=timeout)) def joint_speeds(self) -> list[float] | None: """Current joint speeds in steps per second. @@ -851,9 +851,9 @@ def checkpoint(self, label: str) -> int: def wait_checkpoint(self, label: str, timeout: float = 30.0) -> bool: return _run(self._inner.wait_checkpoint(label, timeout=timeout)) - def write_io(self, index: int, value: int) -> int: + def write_io(self, index: int, value: int, *, timeout: float | None = None) -> int: """Set digital output by logical index (0 = first output pin).""" - return _run(self._inner.write_io(index, value)) + return _run(self._inner.write_io(index, value, timeout=timeout)) def delay(self, seconds: float) -> int: """Insert a non-blocking delay in the motion queue.""" diff --git a/pyproject.toml b/pyproject.toml index 0bc318d..c5d7648 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,7 +46,7 @@ dependencies = [ "psutil>=5.9", "msgspec>=0.18", "ormsgpack>=1.4.0", - "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.15.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.16.0", ] [tool.setuptools.packages.find] diff --git a/tests/integration/test_digital_io.py b/tests/integration/test_digital_io.py new file mode 100644 index 0000000..1a1fb5d --- /dev/null +++ b/tests/integration/test_digital_io.py @@ -0,0 +1,50 @@ +"""Digital I/O uses logical output indices and per-call reply deadlines.""" + +import asyncio +import socket +import time + +import pytest +from waldoctl.skills import skill + +from parol6 import AsyncRobotClient + + +def test_digital_io_readback_and_missing_peer_deadlines(client, server_proc): + before = client.io(timeout=2) + assert before is not None + try: + assert client.write_io(0, 1 - before[2], timeout=2) >= 0 + assert client.wait_status(lambda s: s.io[2] == 1 - before[2], timeout=2) + assert client.io(timeout=2)[2] == 1 - before[2] + finally: + client.write_io(0, before[2], timeout=2) + + async def missing_peer(): + @skill(id="test.io_deadline", version="1.0.0") + async def query(rbt): + return await rbt.io(timeout=0.05) + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as silent: + silent.bind(("127.0.0.1", 0)) + async with AsyncRobotClient( + port=silent.getsockname()[1], timeout=5, retries=3 + ) as absent: + start = time.monotonic() + assert await query.async_call(absent) is None + assert time.monotonic() - start < 1.0, ( + "query ignored its per-call deadline" + ) + start = time.monotonic() + with pytest.raises(TimeoutError): + await absent.write_io(0, 1, timeout=0.05) + assert time.monotonic() - start < 1.0, ( + "write ignored its per-call deadline" + ) + for invalid in (0, -1, float("nan"), float("inf"), True): + with pytest.raises(ValueError): + await absent.io(timeout=invalid) + with pytest.raises(ValueError): + await absent.write_io(0, 1, timeout=invalid) + + asyncio.run(missing_peer()) From 8d3a266f816b1913735de44bc19e6b18350d7467 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 05:05:59 +0000 Subject: [PATCH 2/5] Drop stale replies before each request so a lapsed deadline cannot skew later queries A per-call deadline cancels a query after its datagram was sent; the reply then sits in the receive queue and, with no request ids on the wire, is handed to the next query of any type (returning None for its typed result and leaving every later query one reply behind) or, for a lapsed write_io, lets a stale index-less OK stand in for the next command's ack. Queued replies are discarded when a new request goes out, and a per-call deadline now bounds a single attempt instead of cancelling mid-receive. Co-Authored-By: Claude Fable 5.1 --- parol6/client/async_client.py | 49 ++++++++++++++++++++++------ tests/integration/test_digital_io.py | 29 ++++++++++++++++ 2 files changed, 68 insertions(+), 10 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index b159124..4f71f88 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -588,7 +588,34 @@ 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. @@ -596,6 +623,8 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None: 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. @@ -606,11 +635,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( @@ -637,7 +669,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 @@ -657,6 +689,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: @@ -869,12 +902,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]. diff --git a/tests/integration/test_digital_io.py b/tests/integration/test_digital_io.py index 1a1fb5d..f5117aa 100644 --- a/tests/integration/test_digital_io.py +++ b/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()) From 1901579f1de2a7d51d040bcbb06838e105bbd90d Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 06:42:23 +0000 Subject: [PATCH 3/5] Keep the per-call I/O deadline over endpoint setup as well as the reply The per-call deadline moved inside _request, which no longer bounded endpoint setup and its retries: an io() against an absent peer waited the client's full retry budget. The outer deadline is back around the call; the inner one still keeps the query to a single attempt. Co-Authored-By: Claude Fable 5.1 --- parol6/client/async_client.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 4f71f88..f12f473 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -902,7 +902,13 @@ 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") - resp = await self._request(IOCmd(), timeout=timeout) + # The outer deadline also bounds endpoint setup and its retries on an + # absent peer; the inner one keeps the query to a single attempt. + try: + async with asyncio.timeout(timeout): + resp = await self._request(IOCmd(), timeout=timeout) + except TimeoutError: + return None return resp.io if isinstance(resp, IOResultStruct) else None async def joint_speeds(self) -> list[float] | None: From a2e8194549ddf44b64b7bcffc7f8061a1227e4c2 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 06:44:09 +0000 Subject: [PATCH 4/5] Let the lapsed-deadline reply land before the next query in the stale-reply test Co-Authored-By: Claude Fable 5.1 --- tests/integration/test_digital_io.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tests/integration/test_digital_io.py b/tests/integration/test_digital_io.py index f5117aa..7e48c9c 100644 --- a/tests/integration/test_digital_io.py +++ b/tests/integration/test_digital_io.py @@ -72,8 +72,11 @@ async def scenario(): 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 + # A deadline that lapses mid-flight: the reply lands afterwards + # and must be dropped before the next query goes out. + assert await rbt.io(timeout=1e-4) is None + await asyncio.sleep(0.1) + pose = await rbt.pose() + assert pose is not None and len(pose) == 6 asyncio.run(scenario()) From 9cf5e45e5c464a38518f8033a0866376bbe076c9 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 20:52:40 +0000 Subject: [PATCH 5/5] Correlate every parol6 reply with its request and version the protocol A command datagram now carries a 4-byte request id ahead of the msgpack body and every OK, ERROR and RESPONSE echoes it, so the client matches a reply to the request that is waiting for it and drops one whose caller has already given up. That replaces the heuristic drain, which could only guess from a datagram's type that nobody was waiting for it. Streamed motion sends id 0 and asks for no reply. Status broadcasts carry PROTO_VERSION in their second slot. A client reading a status from another version raises ProtocolVersionError naming both versions instead of failing to decode and reporting silence, which reads as an unplugged arm. Client and controller are released together, so there is no compatibility path for the older layout. The readiness probe went through the codec as well: it hand-built a PING datagram, which the controller stopped parsing the moment the envelope changed, making a live controller read as an absent one. Co-Authored-By: Claude Opus 5 --- README.md | 7 + parol6/client/async_client.py | 85 ++++---- parol6/commands/base.py | 6 +- parol6/commands/query_commands.py | 238 ++++++++++------------ parol6/protocol/wire.py | 200 +++++++++--------- parol6/robot.py | 24 ++- parol6/server/controller.py | 74 ++++--- tests/integration/test_digital_io.py | 12 +- tests/integration/test_udp_smoke.py | 38 ++-- tests/unit/test_messages.py | 47 +++-- tests/unit/test_protocol_version.py | 81 ++++++++ tests/unit/test_query_commands_actions.py | 21 +- 12 files changed, 479 insertions(+), 354 deletions(-) create mode 100644 tests/unit/test_protocol_version.py diff --git a/README.md b/README.md index c28f699..818e552 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,13 @@ This package provides: - **`parol6-server`** CLI for standalone controller operation The controller speaks a msgpack-based UDP protocol and can run on the same machine or remotely. +Every command datagram carries a 4-byte request id ahead of the msgpack body, and the +OK / ERROR / RESPONSE reply echoes it, so a reply whose caller has already given up is +dropped instead of answering the next request. An id of 0 asks for no reply, which is +what streamed motion sends. Status broadcasts carry `PROTO_VERSION` in their second +slot: a client reading a status from another version raises `ProtocolVersionError` +naming both, rather than reporting the silence of a failed decode. Client and +controller are released together — there is no compatibility window between versions. --- diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index f12f473..168f19a 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -110,6 +110,8 @@ ToolStatusResultStruct, ToolsCmd, WriteIOCmd, + MAX_REQ_ID, + ProtocolVersionError, decode_message, encode_command, encode_command_into, @@ -236,7 +238,15 @@ def datagram_received(self, data: bytes, addr: tuple[str, int]) -> None: if self._client._closed: return # Zero-allocation decode directly into shared buffer - if decode_status_bin_into(data, self._client._shared_status): + try: + fresh = decode_status_bin_into(data, self._client._shared_status) + except ProtocolVersionError as mismatch: + # Raising inside a datagram callback reaches nobody. Hold it for + # whoever reads status next, and wake them now. + self._client._proto_error = mismatch + self._client._status_event.set() + return + if fresh: self._client._status_generation += 1 # Event.set() is synchronous, so it's safe to wake waiters from this callback self._client._status_event.set() @@ -299,6 +309,9 @@ def __init__( # Single shared buffer with event-based notification self._status_transport: asyncio.DatagramTransport | None = None self._status_sock: socket.socket | None = None + self._proto_error: ProtocolVersionError | None = None + #: Correlates each reply with its request; 0 means "no reply wanted". + self._next_req_id = 1 self._shared_status: StatusBuffer = StatusBuffer() self._status_generation: int = 0 self._status_event: asyncio.Event = asyncio.Event() @@ -531,6 +544,7 @@ async def stream_status_shared(self) -> AsyncIterator[StatusBuffer]: last_gen = 0 while not self._closed: + self._check_protocol() # Clear before waiting - only affects future waits, not current waiters self._status_event.clear() @@ -548,6 +562,16 @@ async def stream_status_shared(self) -> AsyncIterator[StatusBuffer]: last_gen = self._status_generation yield self._shared_status + def _request_id(self) -> int: + """The next request id, wrapping past the wire's 32-bit field.""" + req_id = self._next_req_id + self._next_req_id = req_id + 1 if req_id < MAX_REQ_ID else 1 + return req_id + + def _check_protocol(self) -> None: + if self._proto_error is not None: + raise self._proto_error + async def _send(self, cmd: msgspec.Struct) -> int: """ Send a binary command based on AckPolicy. @@ -565,16 +589,22 @@ async def _send(self, cmd: msgspec.Struct) -> int: # System commands need stable bytes across the await, so encode a fresh buffer if cmd_type in SYSTEM_CMD_TYPES: + req_id = self._request_id() try: - await self._request_ok_raw(encode_command(cmd), self.timeout) + await self._request_ok_raw( + encode_command(cmd, req_id), self.timeout, req_id + ) return 1 except TimeoutError: return 0 if cmd_type not in QUERY_CMD_TYPES: if self._ack_policy.requires_ack(cmd_type): + req_id = self._request_id() try: - ok = await self._request_ok_raw(encode_command(cmd), self.timeout) + ok = await self._request_ok_raw( + encode_command(cmd, req_id), self.timeout, req_id + ) self._last_command_index = ok.index return ok.index if ok.index is not None else 0 except TimeoutError: @@ -588,31 +618,6 @@ async def _send(self, cmd: msgspec.Struct) -> int: self._transport.sendto(self._tx_buf) return 1 - 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: @@ -634,13 +639,13 @@ async def _request( """ await self._ensure_endpoint() assert self._transport is not None - data = encode_command(cmd) wait = self.timeout if timeout is None else timeout attempts = self.retries + 1 if timeout is None else 1 for attempt in range(attempts): + req_id = self._request_id() + data = encode_command(cmd, req_id) try: async with self._req_lock: - self._drop_stale_replies() self._transport.sendto(data) end_time = time.monotonic() + wait while time.monotonic() < end_time: @@ -651,6 +656,12 @@ async def _request( ) try: parsed = decode_message(resp_data) + if parsed.req_id != req_id: + # A reply to a request whose caller has + # given up. Answering this one with it + # would leave every later query a reply + # behind. + continue if isinstance(parsed, ResponseMsg): return parsed.result if isinstance(parsed, ErrorMsg): @@ -674,13 +685,15 @@ async def _request( await asyncio.sleep(backoff) return None - async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg: + async def _request_ok_raw(self, data: bytes, timeout: float, req_id: int) -> OkMsg: """ - Send pre-encoded binary command and wait for 'OK' or 'ERROR' reply. + Send pre-encoded binary command and wait for the 'OK' or 'ERROR' reply + carrying *req_id*; replies to abandoned requests are discarded. Args: - data: Pre-encoded msgpack bytes + data: Pre-encoded command datagram, id header included timeout: Timeout in seconds. + req_id: The id *data* carries, echoed by the reply. Returns OkMsg on OK; raises RuntimeError on ERROR, TimeoutError on timeout. """ @@ -689,7 +702,6 @@ 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: @@ -699,9 +711,9 @@ async def _request_ok_raw(self, data: bytes, timeout: float) -> OkMsg: ) try: match decode_message(resp_data): - case OkMsg() as ok: + case OkMsg(reply_id) as ok if reply_id == req_id: return ok - case ErrorMsg(message): + case ErrorMsg(reply_id, message) if reply_id == req_id: raise MotionError(RobotError.from_wire(message)) except msgspec.ValidationError: pass # Ignore non-matching datagrams @@ -1419,6 +1431,7 @@ async def wait_status( end_time = time.monotonic() + timeout while time.monotonic() < end_time and not self._closed: + self._check_protocol() self._status_event.clear() # Check if we already have new data diff --git a/parol6/commands/base.py b/parol6/commands/base.py index 02e4716..9bb69cd 100644 --- a/parol6/commands/base.py +++ b/parol6/commands/base.py @@ -11,7 +11,7 @@ import numpy as np from parol6.config import TRACE -from parol6.protocol.wire import CmdType, Command, CommandCode, QueryType +from parol6.protocol.wire import CmdType, Command, CommandCode, QueryType, Response from parol6.server.state import ControllerState from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error from parol6.utils.error_codes import ErrorCode @@ -254,8 +254,8 @@ class QueryCommand(CommandBase[P]): QUERY_TYPE: ClassVar[QueryType] @abstractmethod - def compute(self, state: ControllerState) -> bytes: - """Compute the query result, pack it, and return response bytes.""" + def compute(self, state: ControllerState) -> Response: + """The query's typed result; the controller packs it with the request id.""" ... def execute_step(self, state: ControllerState) -> ExecutionStatusCode: diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 7fd8f49..308a1cc 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -31,6 +31,7 @@ ProfileCmd, ProfileResultStruct, QueryType, + Response, QueueCmd, QueueResultStruct, ReachableCmd, @@ -52,7 +53,6 @@ ToolResultStruct, ToolStatusResultStruct, ToolsCmd, - pack_response, ) from parol6.server.command_registry import register_command from parol6.server.state import get_fkine_flat_mm, get_fkine_se3 @@ -72,14 +72,14 @@ class PoseCommand(QueryCommand[PoseCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: frame = self.p.frame or "WRF" if frame == "TRF": T = get_fkine_se3(state) T_inv = np.linalg.inv(T) T_inv[0:3, 3] *= 1000.0 - return pack_response(PoseResultStruct(pose=T_inv.reshape(-1).tolist())) - return pack_response(PoseResultStruct(pose=get_fkine_flat_mm(state).tolist())) + return PoseResultStruct(pose=T_inv.reshape(-1).tolist()) + return PoseResultStruct(pose=get_fkine_flat_mm(state).tolist()) @register_command(CmdType.ANGLES) @@ -91,11 +91,9 @@ class AnglesCommand(QueryCommand[AnglesCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: cfg.steps_to_rad(state.Position_in, self._q_rad_buf) - return pack_response( - AnglesResultStruct(angles=np.rad2deg(self._q_rad_buf).tolist()) - ) + return AnglesResultStruct(angles=np.rad2deg(self._q_rad_buf).tolist()) @register_command(CmdType.IO) @@ -107,8 +105,8 @@ class IOCommand(QueryCommand[IOCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response(IOResultStruct(io=state.InOut_in[:5].tolist())) + def compute(self, state: "ControllerState") -> Response: + return IOResultStruct(io=state.InOut_in[:5].tolist()) @register_command(CmdType.JOINT_SPEEDS) @@ -120,8 +118,8 @@ class JointSpeedsCommand(QueryCommand[JointSpeedsCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response(SpeedsResultStruct(speeds=state.Speed_in.tolist())) + def compute(self, state: "ControllerState") -> Response: + return SpeedsResultStruct(speeds=state.Speed_in.tolist()) @register_command(CmdType.STATUS) @@ -133,27 +131,25 @@ class StatusCommand(QueryCommand[StatusCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: cache = get_cache() cache.update_from_state(state) ts = cache.tool_status - return pack_response( - StatusResultStruct( - pose=cache.pose.tolist(), - angles=cache.angles_deg.tolist(), - speeds=cache.speeds_rad_s.tolist(), - io=cache.io.tolist(), - tool_status=[ - ts.key, - ts.state, - ts.engaged, - ts.part_detected, - ts.fault_code, - list(ts.positions), - list(ts.channels), - ts.variant_key, - ], - ) + return StatusResultStruct( + pose=cache.pose.tolist(), + angles=cache.angles_deg.tolist(), + speeds=cache.speeds_rad_s.tolist(), + io=cache.io.tolist(), + tool_status=[ + ts.key, + ts.state, + ts.engaged, + ts.part_detected, + ts.fault_code, + list(ts.positions), + list(ts.channels), + ts.variant_key, + ], ) @@ -166,24 +162,22 @@ class LoopStatsCommand(QueryCommand[LoopStatsCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: target_hz = 1.0 / max(cfg.INTERVAL_S, 1e-9) mean_hz = (1.0 / state.mean_period_s) if state.mean_period_s > 0.0 else 0.0 - return pack_response( - LoopStatsResultStruct( - target_hz=target_hz, - loop_count=state.loop_count, - overrun_count=state.overrun_count, - mean_period_s=state.mean_period_s, - std_period_s=state.std_period_s, - min_period_s=state.min_period_s, - max_period_s=state.max_period_s, - p95_period_s=state.p95_period_s, - p99_period_s=state.p99_period_s, - mean_hz=mean_hz, - p50_period_s=state.p50_period_s, - p90_period_s=state.p90_period_s, - ) + return LoopStatsResultStruct( + target_hz=target_hz, + loop_count=state.loop_count, + overrun_count=state.overrun_count, + mean_period_s=state.mean_period_s, + std_period_s=state.std_period_s, + min_period_s=state.min_period_s, + max_period_s=state.max_period_s, + p95_period_s=state.p95_period_s, + p99_period_s=state.p99_period_s, + mean_hz=mean_hz, + p50_period_s=state.p50_period_s, + p90_period_s=state.p90_period_s, ) @@ -196,12 +190,10 @@ class StatusRateCommand(QueryCommand[StatusRateCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - StatusRateResultStruct( - hz=state.status_rate_hz, - control_hz=1.0 / max(cfg.INTERVAL_S, 1e-9), - ) + def compute(self, state: "ControllerState") -> Response: + return StatusRateResultStruct( + hz=state.status_rate_hz, + control_hz=1.0 / max(cfg.INTERVAL_S, 1e-9), ) @@ -214,10 +206,8 @@ class PingCommand(QueryCommand[PingCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - PingResultStruct(hardware_connected=int(state.hardware_connected)) - ) + def compute(self, state: "ControllerState") -> Response: + return PingResultStruct(hardware_connected=int(state.hardware_connected)) @register_command(CmdType.TOOLS) @@ -229,10 +219,8 @@ class ToolsCommand(QueryCommand[ToolsCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - ToolResultStruct(tool=state.current_tool, available=list_tools()) - ) + def compute(self, state: "ControllerState") -> Response: + return ToolResultStruct(tool=state.current_tool, available=list_tools()) @register_command(CmdType.TOOL_STATUS) @@ -244,21 +232,19 @@ class ToolStatusCommand(QueryCommand[ToolStatusCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: cache = get_cache() cache.update_from_state(state) ts = cache.tool_status - return pack_response( - ToolStatusResultStruct( - tool_key=ts.key, - state=ts.state, - engaged=ts.engaged, - part_detected=ts.part_detected, - fault_code=ts.fault_code, - positions=list(ts.positions), - channels=list(ts.channels), - variant_key=ts.variant_key, - ) + return ToolStatusResultStruct( + tool_key=ts.key, + state=ts.state, + engaged=ts.engaged, + part_detected=ts.part_detected, + fault_code=ts.fault_code, + positions=list(ts.positions), + channels=list(ts.channels), + variant_key=ts.variant_key, ) @@ -271,14 +257,12 @@ class ActivityCommand(QueryCommand[ActivityCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - CurrentActionResultStruct( - current=state.action_current, - state=state.action_state.name, - next=state.action_next, - params=state.action_params, - ) + def compute(self, state: "ControllerState") -> Response: + return CurrentActionResultStruct( + current=state.action_current, + state=state.action_state.name, + next=state.action_next, + params=state.action_params, ) @@ -291,15 +275,13 @@ class QueueCommand(QueryCommand[QueueCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - QueueResultStruct( - queue=state.queue_nonstreamable, - executing_index=state.executing_command_index, - completed_index=state.completed_command_index, - last_checkpoint=state.last_checkpoint, - queued_duration=state.queued_duration, - ) + def compute(self, state: "ControllerState") -> Response: + return QueueResultStruct( + queue=state.queue_nonstreamable, + executing_index=state.executing_command_index, + completed_index=state.completed_command_index, + last_checkpoint=state.last_checkpoint, + queued_duration=state.queued_duration, ) @@ -312,8 +294,8 @@ class ProfileCommand(QueryCommand[ProfileCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response(ProfileResultStruct(profile=state.motion_profile)) + def compute(self, state: "ControllerState") -> Response: + return ProfileResultStruct(profile=state.motion_profile) @register_command(CmdType.REACHABLE) @@ -325,15 +307,13 @@ class ReachableCommand(QueryCommand[ReachableCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: cache = get_cache() cache.update_from_state(state) - return pack_response( - EnablementResultStruct( - joint_en=cache.joint_en.tolist(), - cart_en_wrf=cache.cart_en_wrf.tolist(), - cart_en_trf=cache.cart_en_trf.tolist(), - ) + return EnablementResultStruct( + joint_en=cache.joint_en.tolist(), + cart_en_wrf=cache.cart_en_wrf.tolist(), + cart_en_trf=cache.cart_en_trf.tolist(), ) @@ -346,12 +326,10 @@ class ErrorCommand(QueryCommand[ErrorCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: error = state.error - return pack_response( - ErrorResultStruct( - error=error.to_wire() if error is not None else None, - ) + return ErrorResultStruct( + error=error.to_wire() if error is not None else None, ) @@ -364,10 +342,10 @@ class TcpSpeedCommand(QueryCommand[TcpSpeedCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: cache = get_cache() cache.update_from_state(state) - return pack_response(TcpSpeedResultStruct(speed=cache.tcp_speed)) + return TcpSpeedResultStruct(speed=cache.tcp_speed) @register_command(CmdType.IS_SIMULATOR) @@ -379,10 +357,10 @@ class IsSimulatorCommand(QueryCommand[IsSimulatorCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: from parol6.server.transports.transport_factory import is_simulation_mode - return pack_response(IsSimulatorResultStruct(active=is_simulation_mode())) + return IsSimulatorResultStruct(active=is_simulation_mode()) @register_command(CmdType.SHAPES) @@ -398,17 +376,15 @@ class ShapesCommand(QueryCommand[ShapesCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: import parol6.PAROL6_ROBOT as PAROL6_ROBOT - return pack_response( - ShapesResultStruct( - installation=[ - ShapeWire(*s.to_wire()) for s in PAROL6_ROBOT.installation_shapes() - ], - program=[ShapeWire(*s.to_wire()) for s in state.shapes], - epoch=state.shapes_version, - ) + return ShapesResultStruct( + installation=[ + ShapeWire(*s.to_wire()) for s in PAROL6_ROBOT.installation_shapes() + ], + program=[ShapeWire(*s.to_wire()) for s in state.shapes], + epoch=state.shapes_version, ) @@ -421,14 +397,12 @@ class TcpOffsetCommand(QueryCommand[TcpOffsetCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: offset = state.tcp_offset_m - return pack_response( - TcpOffsetResultStruct( - x=offset[0] * 1000, - y=offset[1] * 1000, - z=offset[2] * 1000, - ) + return TcpOffsetResultStruct( + x=offset[0] * 1000, + y=offset[1] * 1000, + z=offset[2] * 1000, ) @@ -438,18 +412,16 @@ class TcpTransformCommand(QueryCommand[TcpTransformCmd]): QUERY_TYPE = QueryType.TCP_TRANSFORM __slots__ = () - def compute(self, state: "ControllerState") -> bytes: + def compute(self, state: "ControllerState") -> Response: from math import degrees xyz = state.tcp_offset_m rpy = state.tcp_rotation_rad - return pack_response( - TcpTransformResultStruct( - x=xyz[0] * 1000, - y=xyz[1] * 1000, - z=xyz[2] * 1000, - roll=degrees(rpy[0]), - pitch=degrees(rpy[1]), - yaw=degrees(rpy[2]), - ) + return TcpTransformResultStruct( + x=xyz[0] * 1000, + y=xyz[1] * 1000, + z=xyz[2] * 1000, + roll=degrees(rpy[0]), + pitch=degrees(rpy[1]), + yaw=degrees(rpy[2]), ) diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index a6566c3..e5a7997 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -6,11 +6,17 @@ - Msgpack message types and structs (UDP communication) - Command/response encoding and decoding +Every command datagram is a 4-byte big-endian request id followed by the +msgpack command; 0 means "no reply expected" (streamed motion). A reply +echoes the id so the client matches it to its request and drops the rest. +Status broadcasts carry PROTO_VERSION right after the type code, so a +client can tell an outdated peer from a silent one. + 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, accepted_index, homed, enabled, homing_step, joints_homed, loop_health, drive_faults] -- RESPONSE: [MsgType.RESPONSE, query_type, value] +- OK: [MsgType.OK, req_id, index?] +- ERROR: [MsgType.ERROR, req_id, message] +- STATUS: [MsgType.STATUS, proto_version, 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, homed, enabled, homing_step, joints_homed, loop_health, drive_faults] +- RESPONSE: [MsgType.RESPONSE, req_id, [query_type, ...fields]] - COMMAND: [CmdType.XXX, ...params] """ @@ -31,8 +37,7 @@ from waldoctl.tools import ToolState from parol6.tools import get_registry, list_tools -from parol6.utils.error_catalog import RobotError, make_error -from parol6.utils.error_codes import ErrorCode +from parol6.utils.error_catalog import RobotError logger = logging.getLogger(__name__) @@ -63,6 +68,23 @@ def _enc_hook(obj: object) -> object: # ============================================================================= +#: Bumped on any change to the command envelope, a reply or the STATUS layout. +PROTO_VERSION = 1 +_REQ_ID_BYTES = 4 +MAX_REQ_ID = 2**32 - 1 + + +class ProtocolVersionError(RuntimeError): + """The peer speaks another protocol version; one side needs upgrading.""" + + def __init__(self, server_version: object) -> None: + self.server_version = server_version + super().__init__( + f"parol6 server speaks protocol version {server_version!r}, this " + f"client speaks {PROTO_VERSION}; update the older side" + ) + + class MsgType(IntEnum): """Message type codes for responses.""" @@ -1023,36 +1045,33 @@ def decode_command(data: bytes) -> Command: return _command_decoder.decode(data) -def encode_command(cmd: Command) -> bytes: - """Encode a typed command struct to bytes. - - Args: - cmd: Typed command struct +def encode_command(cmd: Command, req_id: int = 0) -> bytes: + """A command datagram: the request id header, then the msgpack struct. - Returns: - Raw msgpack-encoded bytes + *req_id* 0 asks for no reply (streamed motion); anything else is echoed + on the OK, ERROR or RESPONSE the server answers with. """ - return _encoder.encode(cmd) - - -def encode_command_into(cmd: Command, buf: bytearray) -> bytearray: - """Encode a typed command struct into a pre-allocated bytearray. + return req_id.to_bytes(_REQ_ID_BYTES, "big") + _encoder.encode(cmd) - The buffer is resized to exactly fit the encoded output. - Reuses the same bytearray object across calls to avoid per-send - ``bytes`` allocations on fire-and-forget paths. - Args: - cmd: Typed command struct - buf: Pre-allocated bytearray (will be resized in-place) +def encode_command_into(cmd: Command, buf: bytearray, req_id: int = 0) -> bytearray: + """``encode_command`` into a pre-allocated bytearray, resized to fit, so + fire-and-forget paths allocate no ``bytes`` per send. - Returns: - The same *buf* object, now containing the encoded bytes. + Returns the same *buf* object, now containing the datagram. """ - _encoder.encode_into(cmd, buf) + _encoder.encode_into(cmd, buf, _REQ_ID_BYTES) + buf[:_REQ_ID_BYTES] = req_id.to_bytes(_REQ_ID_BYTES, "big") return buf +def split_request(data: bytes) -> tuple[int, bytes]: + """The request id and the msgpack payload of a command datagram.""" + if len(data) <= _REQ_ID_BYTES: + raise ValueError(f"command datagram of {len(data)} bytes carries no command") + return int.from_bytes(data[:_REQ_ID_BYTES], "big"), data[_REQ_ID_BYTES:] + + # ============================================================================= # Response Structs - Tagged Union for single-pass decode # Wire format: [MsgType.RESPONSE, QueryType.XXX, ...fields] @@ -1342,6 +1361,7 @@ class OkMsg( ): """OK response, optionally carrying a command index for queued commands.""" + req_id: int index: int | None = None @@ -1354,6 +1374,7 @@ class ErrorMsg( ): """Error response carrying a RobotError wire representation.""" + req_id: int message: list @@ -1366,6 +1387,7 @@ class ResponseMsg( ): """Query response carrying a typed result struct.""" + req_id: int result: Response @@ -1398,44 +1420,24 @@ def decode(data: bytes) -> object: return _decoder.decode(data) -# Pre-packed common responses (avoid repeated packing) -OK_PACKED = _encoder.encode(OkMsg()) - -# Cache for common error responses (3x faster for repeated errors) -_UNKNOWN_CMD_ERROR = make_error(ErrorCode.COMM_UNKNOWN_COMMAND) -_QUEUE_FULL_ERROR = make_error(ErrorCode.COMM_QUEUE_FULL) -_ERROR_CACHE: dict[int, bytes] = { - ErrorCode.COMM_UNKNOWN_COMMAND: _encoder.encode( - ErrorMsg(_UNKNOWN_CMD_ERROR.to_wire()) - ), - ErrorCode.COMM_QUEUE_FULL: _encoder.encode(ErrorMsg(_QUEUE_FULL_ERROR.to_wire())), -} - - -def pack_ok() -> bytes: +def pack_ok(req_id: int) -> bytes: """Pack an OK response (no command index).""" - return OK_PACKED + return _encoder.encode(OkMsg(req_id)) -def pack_ok_index(index: int) -> bytes: +def pack_ok_index(index: int, req_id: int) -> bytes: """Pack an OK response with a command index for queued commands.""" - return _encoder.encode(OkMsg(index=index)) - + return _encoder.encode(OkMsg(req_id, index=index)) -def pack_error(error: RobotError) -> bytes: - """Pack an error response: [ERROR, [command_index, code, title, cause, effect, remedy]]. - Common errors are cached by ErrorCode for performance. - """ - cached = _ERROR_CACHE.get(error.code) - if cached is not None: - return cached - return _encoder.encode(ErrorMsg(error.to_wire())) +def pack_error(error: RobotError, req_id: int) -> bytes: + """Pack an error response: [ERROR, req_id, [command_index, code, title, cause, effect, remedy]].""" + return _encoder.encode(ErrorMsg(req_id, error.to_wire())) -def pack_response(result: Response) -> bytes: - """Pack a query response: [RESPONSE, [query_type_tag, ...fields]].""" - return _encoder.encode(ResponseMsg(result)) +def pack_response(result: Response, req_id: int) -> bytes: + """Pack a query response: [RESPONSE, req_id, [query_type_tag, ...fields]].""" + return _encoder.encode(ResponseMsg(req_id, result)) _NO_JOINTS_HOMED: tuple[int, ...] = (0, 0, 0, 0, 0, 0) @@ -1482,6 +1484,7 @@ def pack_status( return ormsgpack.packb( ( MsgType.STATUS, + PROTO_VERSION, pose, angles, speeds, @@ -1701,7 +1704,7 @@ def _apply_homing_progress(buf: StatusBuffer, step: int, bits: list[int]) -> Non def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: """Zero-allocation decode of STATUS message into preallocated buffer. - Message format: [MsgType.STATUS, pose, angles, speeds, io, + Message format: [MsgType.STATUS, proto_version, 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, @@ -1716,35 +1719,40 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: Returns: True if valid STATUS message, False otherwise. + + Raises: + ProtocolVersionError: the producer speaks another protocol version. """ try: msg = _decoder.decode(data) if ( not isinstance(msg, (list, tuple)) - or len(msg) < 17 + or len(msg) < 18 or msg[0] != MsgType.STATUS ): return False - - buf.pose[:] = msg[1] - buf.angles[:] = msg[2] - buf.speeds[:] = msg[3] - buf.io[:] = msg[4] - buf.action_current = msg[5] - buf.action_state = ActionState(msg[6]) - buf.joint_en[:] = msg[7] - buf.cart_en_wrf[:] = msg[8] - buf.cart_en_trf[:] = msg[9] - buf.executing_index = msg[10] - buf.completed_index = msg[11] - buf.last_checkpoint = msg[12] - raw_error = msg[13] + if msg[1] != PROTO_VERSION: + raise ProtocolVersionError(msg[1]) + + buf.pose[:] = msg[2] + buf.angles[:] = msg[3] + buf.speeds[:] = msg[4] + buf.io[:] = msg[5] + buf.action_current = msg[6] + buf.action_state = ActionState(msg[7]) + buf.joint_en[:] = msg[8] + buf.cart_en_wrf[:] = msg[9] + buf.cart_en_trf[:] = msg[10] + buf.executing_index = msg[11] + buf.completed_index = msg[12] + buf.last_checkpoint = msg[13] + raw_error = msg[14] buf.error = RobotError.from_wire(raw_error) if raw_error is not None else None - buf.queued_segments = msg[14] - buf.queued_duration = msg[15] - buf.action_params = msg[16] + buf.queued_segments = msg[15] + buf.queued_duration = msg[16] + buf.action_params = msg[17] - raw_ts = msg[17] if len(msg) > 17 else None + raw_ts = msg[18] if len(msg) > 18 else None ts = buf.tool_status if ( raw_ts is not None @@ -1763,48 +1771,52 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: ts.positions = tuple(raw_ts[5]) if raw_ts[5] else () ts.channels = tuple(raw_ts[6]) if raw_ts[6] else () - if len(msg) > 18: - buf.tcp_speed = float(msg[18]) - if len(msg) > 19: - buf.simulator_active = bool(msg[19]) + buf.tcp_speed = float(msg[19]) + + if len(msg) > 20: + buf.simulator_active = bool(msg[20]) # Collision viz (appended after simulator_active; len-guarded for # backward-compat with pre-collision status producers). - if len(msg) > 20: - buf.collision_active = bool(msg[20]) if len(msg) > 21: - raw_pairs = msg[21] + buf.collision_active = bool(msg[21]) + if len(msg) > 22: + raw_pairs = msg[22] cp = buf.collision_pairs cp.clear() if raw_pairs: for p in raw_pairs: 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 - buf.homed = bool(msg[24]) if len(msg) > 24 else True - buf.enabled = bool(msg[25]) if len(msg) > 25 else True - if len(msg) > 27: - _apply_homing_progress(buf, int(msg[26]), msg[27]) + if len(msg) > 23: + buf.scene_epoch = int(msg[23]) + buf.accepted_index = int(msg[24]) if len(msg) > 24 else -1 + buf.homed = bool(msg[25]) if len(msg) > 25 else True + buf.enabled = bool(msg[26]) if len(msg) > 26 else True if len(msg) > 28: - lh = msg[28] + _apply_homing_progress(buf, int(msg[27]), msg[28]) + if len(msg) > 29: + lh = msg[29] buf.loop_health = { "p99_period_s": float(lh[0]), "overruns": int(lh[1]), } - if len(msg) > 29: + if len(msg) > 30: # One label tuple per joint, empty when that drive is healthy. # Absent entirely from producers that predate the field, which is # what tells a consumer this backend reports no drive faults at # all rather than reporting all-clear. Replaced wholesale only on # change, so a snapshot's shallow dict copy keeps the labels that # were current when it was taken. - faults = [tuple(f) for f in msg[29]] + faults = [tuple(f) for f in msg[30]] if buf.drive_health.get("faults") != faults: buf.drive_health["faults"] = faults return True + except ProtocolVersionError: + # A version mismatch is the one decode failure that is not a malformed + # datagram: the caller has to hear about it rather than see silence. + raise except Exception as e: logger.debug("decode_status_bin_into: %s", e) return False diff --git a/parol6/robot.py b/parol6/robot.py index 7b0a2ff..fe97f16 100644 --- a/parol6/robot.py +++ b/parol6/robot.py @@ -21,6 +21,7 @@ from pathlib import Path from typing import Any, Literal +import msgspec import numpy as np from numpy.typing import NDArray from pinokin import Robot as PinokinRobot @@ -52,7 +53,7 @@ from parol6.client.sync_client import RobotClient as SyncRobotClient from parol6.config import HOME_ANGLES_DEG, LIMITS from parol6.motion.trajectory import ProfileType -from parol6.protocol.wire import CmdType, MsgType, decode, encode +from parol6.protocol.wire import PingCmd, ResponseMsg, decode_message, encode_command from parol6.tools import ( ElectricGripperConfig, PneumaticGripperConfig, @@ -80,20 +81,21 @@ def _is_server_running( port: int = 5001, timeout: float = 1.0, ) -> bool: - """Return True if a PAROL6 controller responds to UDP PING at host:port.""" + """Return True if a PAROL6 controller responds to UDP PING at host:port. + + Through the codec, not a hand-built datagram: the readiness probe has to + speak exactly what the controller parses, or a live controller reads as + an absent one. + """ + req_id = 1 try: with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(timeout) - ping_msg = encode((CmdType.PING,)) - sock.sendto(ping_msg, (host, port)) + sock.sendto(encode_command(PingCmd(), req_id), (host, port)) data, _ = sock.recvfrom(1024) - resp = decode(data) - return ( - isinstance(resp, (list, tuple)) - and len(resp) >= 1 - and resp[0] == MsgType.RESPONSE - ) - except (OSError, socket.timeout): + reply = decode_message(data) + return isinstance(reply, ResponseMsg) and reply.req_id == req_id + except (OSError, socket.timeout, msgspec.MsgspecError): return False diff --git a/parol6/server/controller.py b/parol6/server/controller.py index aecd433..13f7813 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -38,6 +38,8 @@ pack_error, pack_ok, pack_ok_index, + pack_response, + split_request, unpack_rx_frame_into, ) from parol6.utils.error_catalog import RobotError, extract_robot_error, make_error @@ -612,20 +614,22 @@ def _poll_commands(self, state: ControllerState) -> None: for data, addr in msgs: self._process_command(data, addr, state) - def _reply_error(self, addr: tuple[str, int], error: RobotError) -> None: + def _reply_error( + self, req_id: int, addr: tuple[str, int], error: RobotError + ) -> None: """Send error response to client. Caller must ensure udp_transport is not None.""" assert self.udp_transport is not None - self.udp_transport.send(pack_error(error), addr) + self.udp_transport.send(pack_error(error, req_id), addr) - def _reply_ok(self, addr: tuple[str, int]) -> None: + def _reply_ok(self, req_id: int, addr: tuple[str, int]) -> None: """Send OK response to client. Caller must ensure udp_transport is not None.""" assert self.udp_transport is not None - self.udp_transport.send(pack_ok(), addr) + self.udp_transport.send(pack_ok(req_id), addr) - def _reply_ok_index(self, addr: tuple[str, int], index: int) -> None: + def _reply_ok_index(self, req_id: int, addr: tuple[str, int], index: int) -> None: """Send OK response with command index. Caller must ensure udp_transport is not None.""" assert self.udp_transport is not None - self.udp_transport.send(pack_ok_index(index), addr) + self.udp_transport.send(pack_ok_index(index, req_id), addr) def _process_command( self, data: bytes, addr: tuple[str, int], state: ControllerState @@ -638,9 +642,14 @@ def _process_command( state: Controller state """ self._cmd_rate.record(time.perf_counter()) + try: + req_id, payload = split_request(data) + except ValueError as e: + logger.warning("Dropped datagram from %s: %s", addr, e) + return # Try stream fast-path first (avoids full command creation) - result = self._executor.try_stream_fast_path(data, state) + result = self._executor.try_stream_fast_path(payload, state) if result is True: return @@ -648,17 +657,21 @@ def _process_command( if result is not False: command, category, error = create_command_from_struct(result) else: - command, category, error = create_command(data) + command, category, error = create_command(payload) if not command or category is None: if error: logger.warning(f"Command validation failed: {error}") self._reply_error( - addr, make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=error) + req_id, + addr, + make_error(ErrorCode.COMM_VALIDATION_ERROR, detail=error), ) else: logger.warning("Unknown command") - self._reply_error(addr, make_error(ErrorCode.COMM_UNKNOWN_COMMAND)) + self._reply_error( + req_id, addr, make_error(ErrorCode.COMM_UNKNOWN_COMMAND) + ) return cmd_name = type(command).__name__ @@ -667,14 +680,18 @@ def _process_command( # Dispatch by category (determined at registration time, no isinstance needed) match category: case CommandCategory.QUERY: - self._handle_query(command, state, addr) # type: ignore[arg-type] + self._handle_query(command, state, addr, req_id) # type: ignore[arg-type] case CommandCategory.SYSTEM: - self._handle_system_command(command, state, addr) # type: ignore[arg-type] + self._handle_system_command(command, state, addr, req_id) # type: ignore[arg-type] case CommandCategory.MOTION: - self._handle_motion_command(command, state, addr) # type: ignore[arg-type] + self._handle_motion_command(command, state, addr, req_id) # type: ignore[arg-type] def _handle_motion_command( - self, command: MotionCommand, state: ControllerState, addr: tuple[str, int] + self, + command: MotionCommand, + state: ControllerState, + addr: tuple[str, int], + req_id: int, ) -> None: """Queue motion command for execution.""" cmd_name = type(command).__name__ @@ -684,7 +701,9 @@ def _handle_motion_command( if cmd_type and self._ack_policy.requires_ack(cmd_type): reason = state.disabled_reason or "Controller disabled" self._reply_error( - addr, make_error(ErrorCode.SYS_CONTROLLER_DISABLED, detail=reason) + req_id, + addr, + make_error(ErrorCode.SYS_CONTROLLER_DISABLED, detail=reason), ) logger.warning( "Motion command rejected - controller disabled: %s", cmd_name @@ -715,10 +734,12 @@ def _handle_motion_command( 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) + self._reply_ok_index(req_id, addr, cmd_index) except QueueFullError: if cmd_type and self._ack_policy.requires_ack(cmd_type): - self._reply_error(addr, make_error(ErrorCode.COMM_QUEUE_FULL)) + self._reply_error( + req_id, addr, make_error(ErrorCode.COMM_QUEUE_FULL) + ) return # Tool actions bypass planner — execute directly via side channel @@ -736,6 +757,7 @@ def _handle_motion_command( logger.error("Failed to create tool command: %s", error_msg) if cmd_type and self._ack_policy.requires_ack(cmd_type): self._reply_error( + req_id, addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=error_msg or ""), ) @@ -749,7 +771,7 @@ def _handle_motion_command( TRACE, "Command %s → tool side channel (index=%d)", cmd_name, cmd_index ) if cmd_type and self._ack_policy.requires_ack(cmd_type): - self._reply_ok_index(addr, cmd_index) + self._reply_ok_index(req_id, addr, cmd_index) return # Non-streaming commands → planner @@ -786,24 +808,25 @@ def _handle_motion_command( ) ) if cmd_type and self._ack_policy.requires_ack(cmd_type): - self._reply_ok_index(addr, cmd_index) + self._reply_ok_index(req_id, addr, cmd_index) def _handle_query( self, command: QueryCommand, state: ControllerState, addr: tuple[str, int], + req_id: int, ) -> None: """Execute query command and send response directly.""" try: command.setup(state) - response = command.compute(state) + response = pack_response(command.compute(state), req_id) assert self.udp_transport is not None self.udp_transport.send(response, addr) except Exception as e: logger.error("Query error: %s", e) self._reply_error( - addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=str(e)) + req_id, addr, make_error(ErrorCode.COMM_DECODE_ERROR, detail=str(e)) ) def _resync_planner(self, state: ControllerState) -> None: @@ -827,6 +850,7 @@ def _handle_system_command( command: SystemCommand, state: ControllerState, addr: tuple[str, int], + req_id: int, ) -> None: """Execute system command, apply side effects, and send reply.""" try: @@ -889,17 +913,19 @@ def _handle_system_command( self._planner.sync_shapes(state.shapes) if code == ExecutionStatusCode.COMPLETED: - self._reply_ok(addr) + self._reply_ok(req_id, addr) else: robot_error = command.robot_error or make_error( ErrorCode.MOTN_TICK_FAILED, detail="System command failed" ) - self._reply_error(addr, robot_error) + self._reply_error(req_id, addr, robot_error) except Exception as e: logger.error("System command error: %s", e) self._reply_error( - addr, extract_robot_error(e, ErrorCode.MOTN_SETUP_FAILED, detail=str(e)) + req_id, + addr, + extract_robot_error(e, ErrorCode.MOTN_SETUP_FAILED, detail=str(e)), ) def _assign_command_index(self, state: ControllerState) -> int: diff --git a/tests/integration/test_digital_io.py b/tests/integration/test_digital_io.py index 7e48c9c..7fb6671 100644 --- a/tests/integration/test_digital_io.py +++ b/tests/integration/test_digital_io.py @@ -51,9 +51,10 @@ async def query(rbt): 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.""" + """A reply that lands after its caller's deadline expired is not served to + the next request: it carries the abandoned request's id, so the client + drops it instead of answering the wrong query and leaving every later one + a reply behind.""" from parol6.protocol.wire import IOResultStruct, pack_ok, pack_response async def scenario(): @@ -62,13 +63,14 @@ async def scenario(): ) as rbt: await rbt._ensure_endpoint() peer = (ports.server_ip, ports.server_port) + abandoned = 10_000 # an id no live request will be given rbt._rx_queue.put_nowait( - (pack_response(IOResultStruct(io=[0, 0, 0, 0, 1])), peer) + (pack_response(IOResultStruct(io=[0, 0, 0, 0, 1]), abandoned), 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)) + rbt._rx_queue.put_nowait((pack_ok(abandoned), 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) diff --git a/tests/integration/test_udp_smoke.py b/tests/integration/test_udp_smoke.py index c631eae..774b4ca 100644 --- a/tests/integration/test_udp_smoke.py +++ b/tests/integration/test_udp_smoke.py @@ -171,25 +171,31 @@ class TestErrorHandling: """Test error handling and edge cases.""" def test_invalid_command_format(self, server_proc, ports): - """Test server response to invalid binary msgpack commands.""" - from parol6.protocol.wire import MsgType, encode, decode + """A command body the codec cannot read is refused to the id that sent + it, and a datagram with no request id at all is dropped without + unsettling the controller.""" + from parol6.protocol.wire import ErrorMsg, decode_message, encode - # Send invalid command via raw socket with binary msgpack + req_id = 4242 with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: sock.settimeout(2.0) - # Send an array with invalid command type (9999 is not a valid CmdType) - msg = encode([9999, "invalid_param"]) - sock.sendto(msg, (ports.server_ip, ports.server_port)) - - # Expect error response in array format: [MsgType.ERROR, message] - data, _ = sock.recvfrom(1024) - resp = decode(data) - assert isinstance(resp, (list, tuple)) - assert resp[0] == MsgType.ERROR - # resp[1] is a RobotError wire list: [cmd_idx, code, title, cause, effect, remedy] - error_wire = resp[1] - assert isinstance(error_wire, list) - assert any("9999" in str(f) or "Invalid" in str(f) for f in error_wire) + # 9999 is not a CmdType: the envelope is well formed, the body is not. + body = encode([9999, "invalid_param"]) + sock.sendto( + req_id.to_bytes(4, "big") + body, (ports.server_ip, ports.server_port) + ) + + reply = decode_message(sock.recvfrom(1024)[0]) + assert isinstance(reply, ErrorMsg) + assert reply.req_id == req_id, "the refusal must name the request" + # message is a RobotError wire list: [cmd_idx, code, title, cause, …] + assert isinstance(reply.message, list) + assert any("9999" in str(f) or "Invalid" in str(f) for f in reply.message) + + # A datagram too short to carry an id: nothing to reply to. + sock.sendto(b"\x00\x01", (ports.server_ip, ports.server_port)) + with pytest.raises(socket.timeout): + sock.recvfrom(1024) # Server should remain responsive after handling the error client = RobotClient(ports.server_ip, ports.server_port) diff --git a/tests/unit/test_messages.py b/tests/unit/test_messages.py index 03c282e..61d6cc6 100644 --- a/tests/unit/test_messages.py +++ b/tests/unit/test_messages.py @@ -29,27 +29,30 @@ pack_response, pack_status, ) +from parol6.protocol.wire import PROTO_VERSION, ProtocolVersionError class TestPackUnpack: """Test packing and unpacking roundtrips via decode_message.""" def test_pack_ok(self): - msg = decode_message(pack_ok()) + msg = decode_message(pack_ok(7)) assert isinstance(msg, OkMsg) + assert msg.req_id == 7 assert msg.index is None def test_pack_ok_index(self): - msg = decode_message(pack_ok_index(42)) + msg = decode_message(pack_ok_index(42, 7)) assert isinstance(msg, OkMsg) - assert msg.index == 42 + assert (msg.req_id, msg.index) == (7, 42) def test_pack_error(self): error = make_error( ErrorCode.COMM_VALIDATION_ERROR, detail="Something went wrong" ) - msg = decode_message(pack_error(error)) + msg = decode_message(pack_error(error, 7)) assert isinstance(msg, ErrorMsg) + assert msg.req_id == 7 assert isinstance(msg.message, list) from parol6.utils.error_catalog import RobotError @@ -59,15 +62,16 @@ def test_pack_error(self): def test_pack_response(self): msg = decode_message( - pack_response(AnglesResultStruct(angles=[1.0, 2.0, 3.0, 4.0, 5.0, 6.0])) + pack_response(AnglesResultStruct(angles=[1.0, 2.0, 3.0, 4.0, 5.0, 6.0]), 7) ) assert isinstance(msg, ResponseMsg) + assert msg.req_id == 7 assert isinstance(msg.result, AnglesResultStruct) assert msg.result.angles == [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] def test_pack_response_with_numpy(self): arr = np.array([1.0, 2.0, 3.0], dtype=np.float64) - msg = decode_message(pack_response(PoseResultStruct(pose=arr))) + msg = decode_message(pack_response(PoseResultStruct(pose=arr), 7)) assert isinstance(msg, ResponseMsg) assert isinstance(msg.result, PoseResultStruct) assert msg.result.pose == [1.0, 2.0, 3.0] @@ -107,16 +111,17 @@ def test_pack_status_roundtrip(self): ) unpacked = decode(packed) assert unpacked[0] == MsgType.STATUS - assert unpacked[1] == list(pose) - assert unpacked[2] == list(angles) - assert unpacked[5] == "MoveJCommand" - assert unpacked[6] == ActionState.EXECUTING + assert unpacked[1] == PROTO_VERSION + assert unpacked[2] == list(pose) + assert unpacked[3] == list(angles) + assert unpacked[6] == "MoveJCommand" + assert unpacked[7] == ActionState.EXECUTING - # action_params at index 16 - assert unpacked[16] == "speed=50 acc=100" + # action_params at index 17 + assert unpacked[17] == "speed=50 acc=100" # The optional variant follows the original seven tool-status fields. - ts = unpacked[17] + ts = unpacked[18] assert ts[0] == "ssg48" # key assert ts[1] == 2 # state (ToolState.ACTIVE) assert ts[2] is True # engaged @@ -125,8 +130,8 @@ def test_pack_status_roundtrip(self): assert ts[5] == [0.75, 0.25] # positions (tuple -> list via msgpack) assert ts[6] == [5.5, 3.14] # channels (tuple -> list via msgpack) - # tcp_speed at index 18 - assert unpacked[18] == pytest.approx(123.456) + # tcp_speed at index 19 + assert unpacked[19] == pytest.approx(123.456) def test_pack_decode_status_bin_roundtrip(self): """pack_status -> decode_status_bin_into preserves all tool status fields.""" @@ -178,16 +183,24 @@ def test_pack_decode_status_bin_roundtrip(self): assert ts.variant_key == "pinch" assert buf.copy().tool_status.variant_key == "pinch" legacy = decode(packed) - legacy[17] = legacy[17][:7] + legacy[18] = legacy[18][:7] assert decode_status_bin_into(encode(legacy), buf) assert buf.tool_status.variant_key == "", ( "legacy status retained a stale variant" ) for invalid in (False, 42, None, "x" * 129): bad = decode(packed) - bad[17][7] = invalid + bad[18][7] = invalid assert not decode_status_bin_into(encode(bad), buf) + # A producer speaking another protocol version is named, not decoded + # as silence: a consumer that saw nothing would report a dead + # controller and send an operator looking at cables. + other = decode(packed) + other[1] = PROTO_VERSION + 1 + with pytest.raises(ProtocolVersionError, match=str(PROTO_VERSION + 1)): + decode_status_bin_into(encode(other), buf) + def test_invalid_data_raises(self): with pytest.raises(msgspec.ValidationError): decode_message(encode(["not", "a", "valid", "message"])) diff --git a/tests/unit/test_protocol_version.py b/tests/unit/test_protocol_version.py new file mode 100644 index 0000000..53136bb --- /dev/null +++ b/tests/unit/test_protocol_version.py @@ -0,0 +1,81 @@ +"""A status producer speaking another protocol version is named, not silence. + +The client and the controller are released separately, and a field added to +the status layout shifts every slot after it. Before the version travelled on +the wire, an older controller's status simply failed to decode and the client +reported nothing at all — which reads as an unplugged arm and sends an +operator looking at cables instead of at versions. +""" + +from __future__ import annotations + +import asyncio +import socket + +import numpy as np +import pytest +from waldoctl import ActionState + +from parol6 import config as cfg +from parol6.client.async_client import AsyncRobotClient +from parol6.protocol.wire import ( + PROTO_VERSION, + MsgType, + ProtocolVersionError, + decode, + encode, + pack_status, +) + + +def _status(version: int) -> bytes: + """A well-formed status broadcast, relabelled with *version*.""" + packed = pack_status( + np.eye(4, dtype=np.float64).ravel(), + np.zeros(6, dtype=np.float64), + np.zeros(6, dtype=np.float64), + np.zeros(5, dtype=np.uint8), + "", + ActionState.IDLE, + np.ones(12, dtype=np.uint8), + np.ones(12, dtype=np.uint8), + np.ones(12, dtype=np.uint8), + ) + if version == PROTO_VERSION: + return packed + fields = decode(packed) + assert fields[0] == MsgType.STATUS + fields[1] = version + return encode(fields) + + +def test_a_status_from_another_protocol_version_reaches_the_caller(monkeypatch): + monkeypatch.setattr(cfg, "STATUS_TRANSPORT", "UNICAST") + monkeypatch.setattr(cfg, "STATUS_UNICAST_HOST", "127.0.0.1") + + async def scenario() -> None: + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as probe: + probe.bind(("127.0.0.1", 0)) + status_port = probe.getsockname()[1] + monkeypatch.setattr(cfg, "MCAST_PORT", status_port) + # No controller: this client only listens for the status broadcast. + client = AsyncRobotClient(port=status_port + 1, timeout=0.05, retries=0) + try: + await client._ensure_endpoint() + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as producer: + producer.sendto(_status(PROTO_VERSION), ("127.0.0.1", status_port)) + assert await client.wait_status( + lambda s: s.action_state == ActionState.IDLE, timeout=2 + ), "a status of this version is read normally" + + producer.sendto(_status(PROTO_VERSION + 1), ("127.0.0.1", status_port)) + with pytest.raises(ProtocolVersionError, match="update the older side"): + await client.wait_status(lambda s: False, timeout=2) + # It keeps saying so: a program cannot mistake the mismatch for + # a level that has not arrived yet. + with pytest.raises(ProtocolVersionError): + await client.wait_status(lambda s: False, timeout=0.1) + finally: + await client.close() + + asyncio.run(scenario()) diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py index 56075e8..b3b8205 100644 --- a/tests/unit/test_query_commands_actions.py +++ b/tests/unit/test_query_commands_actions.py @@ -11,22 +11,13 @@ from parol6.commands.query_commands import ActivityCommand, QueueCommand from parol6.protocol.wire import ( - CurrentActionResultStruct, ActivityCmd, + CurrentActionResultStruct, QueueCmd, QueueResultStruct, - ResponseMsg, - decode_message, ) -def _unpack_response(data: bytes): - """Decode packed bytes into a typed result struct.""" - msg = decode_message(data) - assert isinstance(msg, ResponseMsg) - return msg.result - - def test_activity_returns_details(): """Test that ACTIVITY compute() returns correct data.""" state = SimpleNamespace( @@ -38,7 +29,7 @@ def test_activity_returns_details(): cmd = ActivityCommand(ActivityCmd()) cmd.setup(state) - result = _unpack_response(cmd.compute(state)) + result = cmd.compute(state) assert isinstance(result, CurrentActionResultStruct) assert result.current == "MoveJPoseCommand" @@ -58,7 +49,7 @@ def test_activity_with_idle_state(): cmd = ActivityCommand(ActivityCmd()) cmd.setup(state) - result = _unpack_response(cmd.compute(state)) + result = cmd.compute(state) assert isinstance(result, CurrentActionResultStruct) assert result.current == "" @@ -79,7 +70,7 @@ def test_queue_returns_details(): cmd = QueueCommand(QueueCmd()) cmd.setup(state) - result = _unpack_response(cmd.compute(state)) + result = cmd.compute(state) assert isinstance(result, QueueResultStruct) assert result.queue == ["MoveJPoseCommand", "HomeCommand", "MoveJCommand"] @@ -101,7 +92,7 @@ def test_queue_with_empty_queue(): cmd = QueueCommand(QueueCmd()) cmd.setup(state) - result = _unpack_response(cmd.compute(state)) + result = cmd.compute(state) assert isinstance(result, QueueResultStruct) assert result.queue == [] @@ -121,7 +112,7 @@ def test_queue_excludes_streamable(): cmd = QueueCommand(QueueCmd()) cmd.setup(state) - result = _unpack_response(cmd.compute(state)) + result = cmd.compute(state) assert isinstance(result, QueueResultStruct) assert "MoveJPoseCommand" in result.queue