diff --git a/README.md b/README.md index 673fbdd..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. --- @@ -451,3 +458,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 d76d165..1d5b30b 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 @@ -109,6 +110,8 @@ ToolStatusResultStruct, ToolsCmd, WriteIOCmd, + MAX_REQ_ID, + ProtocolVersionError, decode_message, encode_command, encode_command_into, @@ -235,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() @@ -257,7 +268,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, @@ -294,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() @@ -526,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() @@ -543,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. @@ -560,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: @@ -583,7 +618,9 @@ 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: + 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. @@ -591,6 +628,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. @@ -600,12 +639,15 @@ 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): + req_id = self._request_id() + data = encode_command(cmd, req_id) try: async with self._req_lock: 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( @@ -614,6 +656,12 @@ async def _request(self, cmd: msgspec.Struct) -> Response | None: ) 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): @@ -632,18 +680,20 @@ 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 - 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. """ @@ -661,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 @@ -850,15 +900,27 @@ 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()) + 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") + # 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: @@ -1373,6 +1435,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 @@ -1851,7 +1914,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, ...]`` @@ -1859,6 +1924,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: @@ -1870,8 +1938,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 28e07c0..6a289da 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -44,6 +44,7 @@ SelectToolCmd, SetTcpOffsetCmd, SetTcpTransformCmd, + WriteIOCmd, TeleportCmd, ToolActionCmd, ) @@ -550,7 +551,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]: @@ -605,6 +613,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 jog_j( self, joint: int = -1, 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/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 dd605b4..579a3ff 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,16 +190,14 @@ class StatusRateCommand(QueryCommand[StatusRateCmd]): __slots__ = () - def compute(self, state: "ControllerState") -> bytes: - return pack_response( - StatusRateResultStruct( - hz=state.status_rate_hz, - # The configured rate, not 1/INTERVAL_S: inverting the - # interval adds float noise to a value `achievable()` and the - # divisor arithmetic treat as exact (1/(1/49) is 49.000000001). - control_hz=float(cfg.CONTROL_RATE_HZ), - servable=cfg.servable_status_rates(), - ) + def compute(self, state: "ControllerState") -> Response: + return StatusRateResultStruct( + hz=state.status_rate_hz, + # The configured rate, not 1/INTERVAL_S: inverting the interval adds + # float noise to a value `achievable()` and the divisor arithmetic + # treat as exact (1/(1/49) is 49.000000001). + control_hz=float(cfg.CONTROL_RATE_HZ), + servable=cfg.servable_status_rates(), ) @@ -218,10 +210,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) @@ -233,10 +223,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) @@ -248,21 +236,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, ) @@ -275,14 +261,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, ) @@ -295,15 +279,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, ) @@ -316,8 +298,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) @@ -329,15 +311,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(), ) @@ -350,12 +330,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, ) @@ -368,10 +346,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) @@ -383,10 +361,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) @@ -402,17 +380,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, ) @@ -425,14 +401,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, ) @@ -442,18 +416,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 2dd0345..7a46f06 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] @@ -1345,6 +1364,7 @@ class OkMsg( ): """OK response, optionally carrying a command index for queued commands.""" + req_id: int index: int | None = None @@ -1357,6 +1377,7 @@ class ErrorMsg( ): """Error response carrying a RobotError wire representation.""" + req_id: int message: list @@ -1369,6 +1390,7 @@ class ResponseMsg( ): """Query response carrying a typed result struct.""" + req_id: int result: Response @@ -1401,44 +1423,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) @@ -1485,6 +1487,7 @@ def pack_status( return ormsgpack.packb( ( MsgType.STATUS, + PROTO_VERSION, pose, angles, speeds, @@ -1704,7 +1707,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, @@ -1719,35 +1722,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 @@ -1766,48 +1774,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 82f5342..9afe953 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 @@ -629,20 +631,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 @@ -655,9 +659,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 @@ -665,17 +674,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__ @@ -684,14 +697,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__ @@ -701,7 +718,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 @@ -732,10 +751,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 @@ -753,6 +774,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 ""), ) @@ -766,7 +788,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 @@ -803,24 +825,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: @@ -844,6 +867,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: @@ -906,17 +930,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/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..7fb6671 --- /dev/null +++ b/tests/integration/test_digital_io.py @@ -0,0 +1,84 @@ +"""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()) + + +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: 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(): + 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) + 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]), 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(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) + # 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()) 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