From 337237f71826c000d3819b0b88faf5c75c79f953 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 4 Sep 2026 21:02:39 +0000 Subject: [PATCH 1/6] Report per-joint drive faults, and make the status rate settable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The drivers report fault bits over the serial link rather than analog temperature or current registers, so drive_health carries faults and nothing else here: one label tuple per joint, empty when that drive is healthy. A list of empty tuples is deliberately not the same as an empty list — the first is an all-clear, the second is a backend with no fault reporting at all, and a display that cannot tell them apart will show a bus it never asked about as healthy. The label view is rebuilt only when a bit moves, so the 100 Hz path allocates nothing while the drives are fine, and the wire entry is replaced only on change so a snapshot's shallow copy keeps the labels current when it was taken. SET_STATUS_RATE makes the broadcast rate a session knob rather than a boot constant, which is what lets a capture or a tuning run get resolution the 50 Hz default cannot. Status is emitted every Nth control tick, so the achievable rates are the divisors of the control rate; the STATUS_RATE query reports that loop rate rather than a list of legal values, letting a caller compute the set itself. A rate that does not divide evenly is refused with a remedy naming the ones that do, never rounded to a neighbour — a capture taken at a rate nobody asked for is wrong in a way nothing reports. The control loop re-derives its broadcast interval when the rate moves instead of capturing it once at startup, and the status cache adopts the new period too, since it differentiates TCP position against it. Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01DsMBr94VMTsLJtPJq7h6A5 --- parol6/ack_policy.py | 2 + parol6/client/async_client.py | 34 +++++- parol6/client/sync_client.py | 15 ++- parol6/commands/query_commands.py | 20 ++++ parol6/commands/utility_commands.py | 42 +++++++ parol6/protocol/wire.py | 69 ++++++++++- parol6/server/controller.py | 11 ++ parol6/server/state.py | 7 ++ parol6/server/status_cache.py | 43 +++++++ parol6/utils/error_catalog.py | 7 ++ parol6/utils/error_codes.py | 1 + parol6/utils/errors.py | 8 ++ tests/integration/test_drive_faults.py | 77 ++++++++++++ tests/integration/test_status_rate.py | 111 ++++++++++++++++++ tests/integration/test_unhomed_motion_gate.py | 6 +- tests/unit/test_collision_enablement.py | 10 +- 16 files changed, 450 insertions(+), 13 deletions(-) create mode 100644 tests/integration/test_drive_faults.py create mode 100644 tests/integration/test_status_rate.py diff --git a/parol6/ack_policy.py b/parol6/ack_policy.py index 5032297..3c8714c 100644 --- a/parol6/ack_policy.py +++ b/parol6/ack_policy.py @@ -14,6 +14,7 @@ CmdType.WRITE_IO, CmdType.SET_TCP_OFFSET, CmdType.SET_SHAPES, + CmdType.SET_STATUS_RATE, } # Query command types (use request/response, not ACK) @@ -36,6 +37,7 @@ CmdType.IS_SIMULATOR, CmdType.TCP_OFFSET, CmdType.SHAPES, + CmdType.STATUS_RATE, } # Streaming commands are fire-and-forget (no ACK needed) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index ac41cd1..d7b2a5e 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -17,7 +17,13 @@ from waldoctl import RobotClient as _RobotClientABC, Shape, ShapeWorld, ToolStatus from msgspec.structs import asdict from waldoctl.shapes import shape_from_wire -from waldoctl.status import ActionState, ActivityResult, LoopStatsResult, ToolResult +from waldoctl.status import ( + ActionState, + ActivityResult, + LoopStatsResult, + StatusRate, + ToolResult, +) from waldoctl.tools import ToolSpec from .. import config as cfg @@ -65,6 +71,9 @@ ReachableCmd, ResetCmd, ResetLoopStatsCmd, + SetStatusRateCmd, + StatusRateCmd, + StatusRateResultStruct, ResetStateCmd, Response, StopCmd, @@ -931,6 +940,29 @@ async def reset_loop_stats(self) -> int: """ return await self._send(ResetLoopStatsCmd()) + async def set_status_rate(self, hz: float) -> int: + """Set the rate the controller broadcasts status at. + + Category: Configuration + + Example: + rbt.set_status_rate(100) + """ + return await self._send(SetStatusRateCmd(hz=float(hz))) + + async def status_rate(self) -> StatusRate | None: + """Current broadcast rate and the control rate it divides. + + Category: Query + + Example: + rate = rbt.status_rate() + """ + resp = await self._request(StatusRateCmd()) + if not isinstance(resp, StatusRateResultStruct): + return None + return StatusRate(hz=resp.hz, control_hz=resp.control_hz) + async def select_tool(self, tool_name: str, variant_key: str = "") -> int: """Set the active end-effector tool on the controller. diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 803edd4..89e84b5 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -14,7 +14,12 @@ from waldoctl.sync_tools import SyncTool from waldoctl import PingResult, ToolStatus -from waldoctl.status import ActivityResult, LoopStatsResult, ToolResult +from waldoctl.status import ( + ActivityResult, + LoopStatsResult, + StatusRate, + ToolResult, +) from waldoctl.types import Axis, Frame from ..protocol.wire import ( @@ -319,6 +324,14 @@ def reset_loop_stats(self) -> int: """Reset control-loop min/max metrics and overrun count.""" return _run(self._inner.reset_loop_stats()) + def set_status_rate(self, hz: float) -> int: + """Set the rate the controller broadcasts status at.""" + return _run(self._inner.set_status_rate(hz)) + + def status_rate(self) -> StatusRate | None: + """Current broadcast rate and the control rate it divides.""" + return _run(self._inner.status_rate()) + def tools(self) -> ToolResult | None: """Current tool and available tools. diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 7918ee6..0f092a2 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -22,6 +22,8 @@ JointSpeedsCmd, LoopStatsCmd, LoopStatsResultStruct, + StatusRateCmd, + StatusRateResultStruct, PingCmd, PingResultStruct, PoseCmd, @@ -182,6 +184,24 @@ def compute(self, state: "ControllerState") -> bytes: ) +@register_command(CmdType.STATUS_RATE) +class StatusRateCommand(QueryCommand[StatusRateCmd]): + """Return the broadcast rate and the control rate it divides.""" + + PARAMS_TYPE = StatusRateCmd + QUERY_TYPE = QueryType.STATUS_RATE + + __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), + ) + ) + + @register_command(CmdType.PING) class PingCommand(QueryCommand[PingCmd]): """Respond to ping requests.""" diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 070d029..18dccc7 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,13 +11,19 @@ MotionCommand, SystemCommand, ) +from parol6.config import CONTROL_RATE_HZ from parol6.protocol.wire import ( CheckpointCmd, CmdType, DelayCmd, ResetLoopStatsCmd, ResetStateCmd, + SetStatusRateCmd, ) +from parol6.server.status_cache import get_cache +from parol6.utils.error_catalog import make_error +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import ConfigurationError from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -89,6 +95,42 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: return ExecutionStatusCode.COMPLETED +@register_command(CmdType.SET_STATUS_RATE) +class SetStatusRateCommand(SystemCommand[SetStatusRateCmd]): + """Change the status broadcast rate for this session. + + Status is emitted every Nth control tick, so a rate that does not divide + the control rate evenly cannot be served. It is refused rather than + rounded to a neighbour: a capture taken at a rate nobody asked for is + wrong in a way nothing reports. + """ + + PARAMS_TYPE = SetStatusRateCmd + + __slots__ = () + + def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: + hz = float(self.p.hz) + control = int(CONTROL_RATE_HZ) + if hz <= 0.0 or hz > control or control % int(hz) != 0 or hz != int(hz): + allowed = ", ".join( + str(control // n) for n in range(1, control + 1) if control % n == 0 + ) + raise ConfigurationError( + make_error( + ErrorCode.SYS_STATUS_RATE_INVALID, + requested=hz, + control=control, + allowed=allowed, + ) + ) + state.status_rate_hz = hz + get_cache().set_status_rate(hz) + logger.info("Status broadcast rate set to %g Hz", hz) + self.finish() + return ExecutionStatusCode.COMPLETED + + @register_command(CmdType.CHECKPOINT) class CheckpointCommand(MotionCommand[CheckpointCmd]): """Queue marker that sets state.last_checkpoint on execution. diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 8a821f7..672aab9 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -9,7 +9,7 @@ Wire format uses msgpack arrays with integer type codes: - OK: MsgType.OK (just the integer) - ERROR: [MsgType.ERROR, message] -- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, loop_health] +- 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] - COMMAND: [CmdType.XXX, ...params] """ @@ -92,6 +92,7 @@ class QueryType(IntEnum): IS_SIMULATOR = auto() TCP_OFFSET = auto() SHAPES = auto() + STATUS_RATE = auto() class CmdType(IntEnum): @@ -159,6 +160,10 @@ class CmdType(IntEnum): SET_SHAPES = auto() # Collision-world readback query (installation + program layers) SHAPES = auto() + # Status broadcast rate: set it for a session, read it back with the + # control rate it divides. + SET_STATUS_RATE = auto() + STATUS_RATE = auto() # ============================================================================= @@ -861,6 +866,30 @@ class ActivityCmd( pass +class StatusRateCmd( + msgspec.Struct, + tag=int(CmdType.STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """STATUS_RATE: [CmdType.STATUS_RATE] — read the broadcast rate.""" + + pass + + +class SetStatusRateCmd( + msgspec.Struct, + tag=int(CmdType.SET_STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """SET_STATUS_RATE: [CmdType.SET_STATUS_RATE, hz]""" + + hz: float + + class LoopStatsCmd( msgspec.Struct, tag=int(CmdType.LOOP_STATS), @@ -997,6 +1026,19 @@ class StatusResultStruct( tool_status: list +class StatusRateResultStruct( + msgspec.Struct, + tag=int(QueryType.STATUS_RATE), + array_like=True, + frozen=True, + gc=False, +): + """Broadcast rate, and the control rate it divides.""" + + hz: float + control_hz: float + + class LoopStatsResultStruct( msgspec.Struct, tag=int(QueryType.LOOP_STATS), @@ -1204,6 +1246,7 @@ class ShapesResultStruct( Response = ( StatusResultStruct | LoopStatsResultStruct + | StatusRateResultStruct | ToolResultStruct | CurrentActionResultStruct | PingResultStruct @@ -1364,6 +1407,7 @@ def pack_status( joints_homed: Sequence[int] = _NO_JOINTS_HOMED, p99_period_s: float = 0.0, overruns: int = 0, + drive_faults: Sequence[Sequence[str]] = (), ) -> bytes: """Pack a status broadcast message. @@ -1412,6 +1456,7 @@ def pack_status( homing_step, joints_homed, (p99_period_s, overruns), + drive_faults, ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1471,9 +1516,10 @@ class StatusBuffer: warnings: list[tuple] = field(default_factory=list) link_health: dict = field(default_factory=dict) # The drives report per-joint error FLAGS over the serial link, not - # analog temperature or current registers, so there is nothing to put - # here: empty is what tells a consumer "no such sensor" rather than - # "all zero". Flags surface as faults, not as a trend. + # analog temperature or current registers, so this carries "faults" + # (one label tuple per joint) and no temperature/current series: empty + # lists are what tell a consumer "no such sensor" rather than "all + # zero", and a list of empty tuples is an all-clear. drive_health: dict = field(default_factory=dict) # The control loop's own health: p99_period_s and overruns. Empty from # producers that predate the field, which is how a consumer tells @@ -1596,7 +1642,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: tool_status_tuple, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, - loop_health] + loop_health, drive_faults] Args: data: Raw msgpack bytes @@ -1677,6 +1723,16 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: "p99_period_s": float(lh[0]), "overruns": int(lh[1]), } + if len(msg) > 29: + # 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]] + if buf.drive_health.get("faults") != faults: + buf.drive_health["faults"] = faults return True except Exception as e: @@ -1950,6 +2006,8 @@ def unpack_rx_frame_into( "QueueCmd", "ActivityCmd", "LoopStatsCmd", + "StatusRateCmd", + "SetStatusRateCmd", "ProfileCmd", "Command", # Mixin @@ -1957,6 +2015,7 @@ def unpack_rx_frame_into( # Response structs "StatusResultStruct", "LoopStatsResultStruct", + "StatusRateResultStruct", "ToolResultStruct", "CurrentActionResultStruct", "PingResultStruct", diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 0f6252c..c80ba3f 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -73,6 +73,7 @@ MCAST_TTL, STATUS_RATE_HZ, STATUS_STALE_S, + CONTROL_RATE_HZ, STATUS_BROADCAST_INTERVAL, ) @@ -538,6 +539,10 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 + # Re-derived rather than captured once: SET_STATUS_RATE moves the rate + # mid-session, and a local snapshot would keep broadcasting at whatever + # the rate was at boot. + broadcast_rate_hz = 0.0 broadcast_interval = STATUS_BROADCAST_INTERVAL while self.running: @@ -558,6 +563,12 @@ def _main_control_loop(self): with pt.phase("execute"): self._execute_commands(state) + if state.status_rate_hz != broadcast_rate_hz: + broadcast_rate_hz = state.status_rate_hz + broadcast_interval = max( + 1, int(CONTROL_RATE_HZ) // int(broadcast_rate_hz) + ) + if tick_count % broadcast_interval == 0: with pt.phase("status"): if self._status_broadcaster: diff --git a/parol6/server/state.py b/parol6/server/state.py index 3a0f8c8..2bc35ac 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -8,6 +8,7 @@ import numpy as np import parol6.PAROL6_ROBOT as PAROL6_ROBOT +from parol6 import config as _cfg from pinokin import arrays_equal_6 from parol6.config import CONTROL_RATE_HZ, steps_to_rad from parol6.motion import CartesianStreamingExecutor, StreamingExecutor @@ -245,6 +246,12 @@ class ControllerState: # firmware to clear the homed bits, 3 waiting for every joint); meaningful # only while action_current is "HomeCommand". homing_step: int = 0 + # Status broadcast rate for this session. Mutable so SET_STATUS_RATE can + # raise it for a capture or a tuning run and drop it back without a + # restart; the control loop re-derives its broadcast interval when this + # moves. Constrained to divisors of CONTROL_RATE_HZ, since status is + # emitted every Nth tick. + status_rate_hz: float = _cfg.STATUS_RATE_HZ action_state: ActionState = ActionState.IDLE # IDLE, EXECUTING, ERROR action_next: str = "" queue_nonstreamable: list[str] = field(default_factory=list) diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 9cb270b..d6c666f 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -200,6 +200,16 @@ def __init__(self) -> None: self._status_rate_hz: float = _cfg.STATUS_RATE_HZ + # Per-joint drive faults. The firmware reports two bitfields; the + # label view is rebuilt only when a bit moves, so the 100 Hz path + # allocates nothing while the drives are healthy. One entry per + # joint always — an all-clear list of empty tuples is how a consumer + # tells "this backend reports faults, none active" from "this + # backend has no fault reporting", which is an empty list. + self._temp_fault_bits = np.zeros(6, dtype=np.uint8) + self._poserr_fault_bits = np.zeros(6, dtype=np.uint8) + self._drive_faults: list[tuple[str, ...]] = [() for _ in range(6)] + # IK enablement results (pre-allocated for zero-alloc reads) self._joint_en = np.ones(12, dtype=np.uint8) self._cart_en_wrf = np.ones(12, dtype=np.uint8) @@ -529,6 +539,25 @@ def update_from_state(self, state: ControllerState) -> None: if enabled_changed: self._enabled = state.enabled + faults_changed = False + for i in range(6): + temp_bit = 1 if state.Temperature_error_in[i] else 0 + pos_bit = 1 if state.Position_error_in[i] else 0 + if self._temp_fault_bits[i] != temp_bit: + self._temp_fault_bits[i] = temp_bit + faults_changed = True + if self._poserr_fault_bits[i] != pos_bit: + self._poserr_fault_bits[i] = pos_bit + faults_changed = True + if faults_changed: + for i in range(6): + labels = [] + if self._temp_fault_bits[i]: + labels.append("overtemperature") + if self._poserr_fault_bits[i]: + labels.append("following_error") + self._drive_faults[i] = tuple(labels) + # Only a live HomeCommand owns homing_step; any cancel path that drops # the command clears action_current, so derive "idle" from that. step = state.homing_step if state.action_current == "HomeCommand" else 0 @@ -580,6 +609,7 @@ def update_from_state(self, state: ControllerState) -> None: or collision_changed or depth_changed or loop_changed + or faults_changed ): self._binary_dirty = True @@ -618,10 +648,23 @@ def to_binary(self) -> bytes: joints_homed=self._joints_homed, p99_period_s=self._p99_period_s, overruns=self._overruns, + drive_faults=self._drive_faults, ) self._binary_dirty = False return self._binary_cache + def set_status_rate(self, hz: float) -> None: + """Adopt a new broadcast rate. + + The cache differentiates TCP position against the broadcast period, + so a rate change that missed this would scale every reported speed. + """ + self._status_rate_hz = hz + + @property + def status_rate_hz(self) -> float: + return self._status_rate_hz + def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" self.last_serial_s = time.monotonic() diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 396d3cd..d9f732a 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -121,6 +121,13 @@ class _ErrorTemplate: remedy="Check parameter ranges and types.", ), # -- System / safety -- + ErrorCode.SYS_STATUS_RATE_INVALID: _ErrorTemplate( + title="Status rate not achievable", + cause="Status is broadcast every Nth control tick, so {requested} Hz " + "does not divide the {control} Hz control loop evenly.", + effect="Broadcast rate unchanged.", + remedy="Pick a rate that divides {control} Hz: {allowed}.", + ), ErrorCode.SYS_CONTROLLER_DISABLED: _ErrorTemplate( title="Controller disabled", cause="Motion command sent while controller is disabled. {detail}", diff --git a/parol6/utils/error_codes.py b/parol6/utils/error_codes.py index bf31cd4..c75a241 100644 --- a/parol6/utils/error_codes.py +++ b/parol6/utils/error_codes.py @@ -40,3 +40,4 @@ class ErrorCode(IntEnum): SYS_PORT_SAVE_FAILED = 52 SYS_PROFILE_INVALID = 53 SYS_SELF_COLLISION = 54 + SYS_STATUS_RATE_INVALID = 55 diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py index a47d793..f2bfa19 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -29,6 +29,14 @@ def __init__(self, robot_error: RobotError): super().__init__(str(robot_error)) +class ConfigurationError(RuntimeError): + """A configuration command the controller cannot honour.""" + + def __init__(self, robot_error: RobotError): + self.robot_error = robot_error + super().__init__(str(robot_error)) + + class MotionError(RuntimeError): """Pipeline planning/execution error detected via status broadcast.""" diff --git a/tests/integration/test_drive_faults.py b/tests/integration/test_drive_faults.py new file mode 100644 index 0000000..71b0ef1 --- /dev/null +++ b/tests/integration/test_drive_faults.py @@ -0,0 +1,77 @@ +"""Per-joint drive faults ride the status broadcast. + +The drivers report fault bits over the serial link rather than analog +temperature or current registers, so this backend's drive health is faults +and nothing else — and an all-clear has to be distinguishable from a +backend that reports no faults at all, or a display cannot tell "healthy" +from "not instrumented". + +Driven through the real cache and the real codec: a ControllerState carrying +the bits the firmware would set, encoded by the cache the broadcaster uses, +decoded by the buffer the client fills. +""" + +import pytest + +from parol6.protocol.wire import StatusBuffer, decode_status_bin_into +from parol6.server.state import ControllerState +from parol6.server.status_cache import StatusCache + + +def _decode(cache: StatusCache) -> StatusBuffer: + buf = StatusBuffer() + assert decode_status_bin_into(cache.to_binary(), buf), "STATUS failed to decode" + return buf + + +@pytest.mark.integration +def test_faults_reach_the_client_against_the_joint_that_tripped(): + """A tripped drive names its condition, on its own joint, and a healthy + bus still reports one entry per joint so all-clear is visible as such.""" + cache = StatusCache() + try: + state = ControllerState() + + cache.update_from_state(state) + healthy = _decode(cache).drive_health["faults"] + assert healthy == [(), (), (), (), (), ()], ( + "a healthy bus must still report one entry per joint, so that " + f"all-clear is distinguishable from no reporting: {healthy}" + ) + + state.Temperature_error_in[2] = 1 + state.Position_error_in[4] = 1 + cache.update_from_state(state) + faults = _decode(cache).drive_health["faults"] + assert faults[2] == ("overtemperature",), faults + assert faults[4] == ("following_error",), faults + assert all(faults[i] == () for i in (0, 1, 3, 5)), ( + f"a fault on one drive must not read as a fault on another: {faults}" + ) + + # Both conditions on one drive, and the earlier fault clearing. + state.Temperature_error_in[2] = 0 + state.Temperature_error_in[4] = 1 + cache.update_from_state(state) + faults = _decode(cache).drive_health["faults"] + assert faults[2] == (), "a cleared fault must stop being reported" + assert set(faults[4]) == {"overtemperature", "following_error"}, faults + finally: + cache.close() + + +@pytest.mark.integration +def test_a_bus_with_no_analog_registers_reports_no_readings(): + """Faults are this backend's only drive health. Empty temperature and + current lists are what tell a consumer there is no such sensor, rather + than a row of zeros that reads as a cold, idle drive.""" + cache = StatusCache() + try: + cache.update_from_state(ControllerState()) + health = _decode(cache).drive_health + assert health.get("faults"), "faults are reported" + assert not health.get("temperatures_c"), health + assert not health.get("currents_ma"), health + assert health.get("bus_voltage_v") is None, health + finally: + cache.close() diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py new file mode 100644 index 0000000..8e58fee --- /dev/null +++ b/tests/integration/test_status_rate.py @@ -0,0 +1,111 @@ +"""The status broadcast rate is a session knob, not a boot constant. + +Raising it is how a capture or a tuning run gets resolution the default +50 Hz cannot give. Status is emitted every Nth control tick, so the rates a +controller can serve are the divisors of its control rate — reported as the +control rate itself, so a caller computes the set rather than probing for +it by rejection. +""" + +import asyncio +import time + +import pytest + +from parol6 import AsyncRobotClient + + +async def _observed_hz(client: AsyncRobotClient, frames: int = 40) -> float: + """Measure arrival rate over *frames* distinct broadcasts.""" + seen = 0 + start = 0.0 + async for _ in client.stream_status(): + if seen == 0: + start = time.perf_counter() + seen += 1 + if seen > frames: + break + return frames / max(time.perf_counter() - start, 1e-9) + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_the_rate_reports_the_loop_it_divides(server_proc, ports): + """``control_hz`` is what makes the constraint computable by a caller: + every rate it implies must actually be accepted.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + + rate = await client.status_rate() + assert rate is not None + assert rate.control_hz > 0.0 + assert rate.hz > 0.0 + assert rate.control_hz % rate.hz == 0.0, ( + f"the controller is broadcasting at {rate.hz} Hz, which does not " + f"divide its own {rate.control_hz} Hz loop" + ) + + # Everything achievable() derives from control_hz must be accepted; + # that is the whole contract of reporting the loop rate instead of a + # list, so it is checked rather than assumed. + for candidate in rate.achievable(): + assert await client.set_status_rate(candidate) > 0, ( + f"{candidate} Hz divides {rate.control_hz} Hz but was refused" + ) + assert await client.set_status_rate(rate.hz) > 0 + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_raising_the_rate_delivers_more_frames(server_proc, ports): + """The point of the knob is resolution, so the change has to show up in + the arrival rate rather than only in the readback.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + original = await client.status_rate() + assert original is not None + + low = original.control_hz / 10 + high = original.control_hz / 2 + try: + assert await client.set_status_rate(low) > 0 + await asyncio.sleep(0.3) + slow = await _observed_hz(client) + + assert await client.set_status_rate(high) > 0 + back = await client.status_rate() + assert back is not None and back.hz == high + await asyncio.sleep(0.3) + fast = await _observed_hz(client) + finally: + await client.set_status_rate(original.hz) + + assert fast > slow * 2, ( + f"asked for {high} Hz after {low} Hz but saw {fast:.1f} vs {slow:.1f}" + ) + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports): + """Refused, never rounded to a neighbour: a capture taken at a rate + nobody asked for is wrong in a way nothing reports. The refusal has to + carry what the legal rates are, since that is all the operator needs.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + before = await client.status_rate() + assert before is not None + + bogus = before.control_hz / 3 + 0.5 + assert bogus not in before.achievable() + + with pytest.raises(Exception) as caught: + await client.set_status_rate(bogus) + message = str(caught.value) + assert str(int(before.control_hz)) in message, message + assert "divide" in message.lower(), message + + after = await client.status_rate() + assert after is not None and after.hz == before.hz, ( + "a refused rate must leave the broadcast alone" + ) diff --git a/tests/integration/test_unhomed_motion_gate.py b/tests/integration/test_unhomed_motion_gate.py index d177f0c..2c5f739 100644 --- a/tests/integration/test_unhomed_motion_gate.py +++ b/tests/integration/test_unhomed_motion_gate.py @@ -54,8 +54,10 @@ def test_home_calibrate_rereferences_homed_robot(client: RobotClient, server_pro assert client.wait_status(lambda s: not s.homed, timeout=5.0) # Progress is published while the firmware seeks the end stops... assert client.wait_status( - lambda s: bool(s.homing.get("active")) - and any(state.name == "SEEKING" for state, _ in s.homing["joints"]), + lambda s: ( + bool(s.homing.get("active")) + and any(state.name == "SEEKING" for state, _ in s.homing["joints"]) + ), timeout=5.0, ) assert client.wait_command(idx, timeout=30.0) diff --git a/tests/unit/test_collision_enablement.py b/tests/unit/test_collision_enablement.py index 763f26b..5f86cd8 100644 --- a/tests/unit/test_collision_enablement.py +++ b/tests/unit/test_collision_enablement.py @@ -94,8 +94,9 @@ def test_gate_greys_direction_entering_new_pair_while_inside(): checker = _FakeChecker( lambda q: -0.01 + q[0], # A dominates; J1+ improves, J1- deepens lambda q: True, - pairs=lambda q: [("L6", "shape:A")] - + ([("L4", "shape:B")] if q[1] > 0.001 else []), + pairs=lambda q: ( + [("L6", "shape:A")] + ([("L4", "shape:B")] if q[1] > 0.001 else []) + ), ) gate_joint_enable_collision(checker, np.zeros(6), joint_en, np.zeros(6)) assert joint_en[0] == 1 # J1+ pure escape -> enabled @@ -122,8 +123,9 @@ def test_collision_blocked_new_pair_blocks_even_when_global_min_improves(): inside = _FakeChecker( lambda q: -0.05 + q[0], # A dominates the global min; +x improves it lambda q: True, - pairs=lambda q: [("L6", "shape:A")] - + ([("L5", "shape:B")] if q[0] > 0.05 else []), + pairs=lambda q: ( + [("L6", "shape:A")] + ([("L5", "shape:B")] if q[0] > 0.05 else []) + ), ) # Pure escape (no new contact) stays allowed… assert collision_blocked(inside, np.zeros(6), np.full(6, 0.01)) is False From 7b04e401a0eb5c7a702817f0bc4be93a12dd9ccd Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sat, 5 Sep 2026 12:53:19 -0400 Subject: [PATCH 2/6] Refuse an unservable status rate without crashing on the way out The validator divided before it screened: control % int(hz) ran ahead of the integrality check, so 0.5 Hz floored to a zero divisor and NaN could not be made an int at all. Both escaped as a generic tick failure, which is the one thing a refusal must not do -- whoever asked for 0.5 Hz got a crash instead of the rates that would have worked. The guard now establishes finite, in range and integral before the modulo sees anything, and refuses through the command protocol rather than an exception the executor has to unwrap. make_error() formatted title and cause only, so the one template that puts its parameter in the remedy -- the remedy being where a refusal says what would have worked -- reached the client reading "Pick a rate that divides {control} Hz: {allowed}". All four fields format now. The rate also had more owners than one. STATUS_BROADCAST_INTERVAL was computed at import from the boot rate, the status cache kept a _status_rate_hz shadow of its own, and StatusBroadcaster took a rate_hz it never read. ControllerState owns it now: status_broadcast_interval(hz) derives the tick count on demand, and the TCP-speed derivative divides by the live broadcast period while keeping the period its previous sample was taken at, so the frame that straddles a rate change is not differentiated against a period it never spanned. Drive-fault labels are a pre-built lookup keyed by the two bits, because that path runs at 100 Hz and the hot path does not allocate. Co-Authored-By: Claude Opus 5 --- parol6/commands/utility_commands.py | 12 +-- parol6/config.py | 12 ++- parol6/server/controller.py | 20 ++--- parol6/server/status_broadcast.py | 6 +- parol6/server/status_cache.py | 69 ++++++++--------- parol6/utils/error_catalog.py | 12 ++- parol6/utils/errors.py | 8 -- .../test_status_broadcast_autofailover.py | 3 +- tests/integration/test_status_rate.py | 77 ++++++++++++++++--- 9 files changed, 139 insertions(+), 80 deletions(-) diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 18dccc7..6ec7821 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -20,10 +20,8 @@ ResetStateCmd, SetStatusRateCmd, ) -from parol6.server.status_cache import get_cache from parol6.utils.error_catalog import make_error from parol6.utils.error_codes import ErrorCode -from parol6.utils.errors import ConfigurationError from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -112,11 +110,15 @@ class SetStatusRateCommand(SystemCommand[SetStatusRateCmd]): def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: hz = float(self.p.hz) control = int(CONTROL_RATE_HZ) - if hz <= 0.0 or hz > control or control % int(hz) != 0 or hz != int(hz): + # Ordered so the modulo only ever sees a finite, in-range, integral + # divisor: int(0.5) is 0 and int(nan) raises, and either would leave + # as a generic tick failure instead of the refusal that names the + # rates this controller can serve. + if not (1.0 <= hz <= control) or not hz.is_integer() or control % int(hz) != 0: allowed = ", ".join( str(control // n) for n in range(1, control + 1) if control % n == 0 ) - raise ConfigurationError( + self.fail( make_error( ErrorCode.SYS_STATUS_RATE_INVALID, requested=hz, @@ -124,8 +126,8 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: allowed=allowed, ) ) + return ExecutionStatusCode.FAILED state.status_rate_hz = hz - get_cache().set_status_rate(hz) logger.info("Status broadcast rate set to %g Hz", hz) self.finish() return ExecutionStatusCode.COMPLETED diff --git a/parol6/config.py b/parol6/config.py index 0f2b657..f3f3046 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -88,13 +88,23 @@ def _trace(self, msg, *args, **kwargs): STATUS_RATE_HZ: float = float(os.getenv("PAROL6_STATUS_RATE_HZ", "50")) STATUS_STALE_S: float = float(os.getenv("PAROL6_STATUS_STALE_S", "0.5")) + +def status_broadcast_interval(hz: float) -> int: + """Control ticks between status broadcasts at *hz*. + + Derived on demand rather than fixed at import: the rate is a session + knob (SET_STATUS_RATE), and a constant computed from the boot rate would + be a second answer to the question of how often status goes out. + """ + return max(1, int(CONTROL_RATE_HZ) // int(hz)) + + # Validate STATUS_RATE_HZ divides evenly into CONTROL_RATE_HZ for polling if int(CONTROL_RATE_HZ) % int(STATUS_RATE_HZ) != 0: raise ValueError( f"STATUS_RATE_HZ ({STATUS_RATE_HZ}) must divide evenly into " f"CONTROL_RATE_HZ ({CONTROL_RATE_HZ})" ) -STATUS_BROADCAST_INTERVAL: int = int(CONTROL_RATE_HZ) // int(STATUS_RATE_HZ) # Max ticks to hold MOVE at trajectory endpoint waiting for Position_in to converge. # At 100Hz control rate, 20 ticks = 200ms. If the robot hasn't reached the target diff --git a/parol6/server/controller.py b/parol6/server/controller.py index c80ba3f..25d06b0 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -71,10 +71,8 @@ MCAST_PORT, MCAST_IF, MCAST_TTL, - STATUS_RATE_HZ, STATUS_STALE_S, - CONTROL_RATE_HZ, - STATUS_BROADCAST_INTERVAL, + status_broadcast_interval, ) import psutil @@ -194,7 +192,7 @@ def _initialize_components(self) -> None: try: logger.debug( - f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} rate_hz={STATUS_RATE_HZ} stale_s={STATUS_STALE_S}" + f"StatusBroadcaster config: group={MCAST_GROUP} port={MCAST_PORT} ttl={MCAST_TTL} iface={MCAST_IF} stale_s={STATUS_STALE_S}" ) self._status_broadcaster = StatusBroadcaster( state_mgr=self.state_manager, @@ -202,7 +200,6 @@ def _initialize_components(self) -> None: port=MCAST_PORT, ttl=MCAST_TTL, iface_ip=MCAST_IF, - rate_hz=STATUS_RATE_HZ, stale_s=STATUS_STALE_S, ) logger.debug("StatusBroadcaster initialized") @@ -539,11 +536,12 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 - # Re-derived rather than captured once: SET_STATUS_RATE moves the rate - # mid-session, and a local snapshot would keep broadcasting at whatever - # the rate was at boot. + # Re-derived from the state rather than captured once: SET_STATUS_RATE + # moves the rate mid-session, and a snapshot taken here would keep + # broadcasting at whatever the rate was at boot. The sentinel rate + # never matches, so the first tick derives the real interval. broadcast_rate_hz = 0.0 - broadcast_interval = STATUS_BROADCAST_INTERVAL + broadcast_interval = 1 while self.running: try: @@ -565,9 +563,7 @@ def _main_control_loop(self): if state.status_rate_hz != broadcast_rate_hz: broadcast_rate_hz = state.status_rate_hz - broadcast_interval = max( - 1, int(CONTROL_RATE_HZ) // int(broadcast_rate_hz) - ) + broadcast_interval = status_broadcast_interval(broadcast_rate_hz) if tick_count % broadcast_interval == 0: with pt.phase("status"): diff --git a/parol6/server/status_broadcast.py b/parol6/server/status_broadcast.py index f967ec6..9754ba6 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -29,8 +29,11 @@ class StatusBroadcaster: - cfg.STATUS_UNICAST_HOST (default "127.0.0.1") General: - - cfg.STATUS_RATE_HZ (default 50) - cfg.STATUS_STALE_S (default 0.2) -> skip broadcast if cache is stale + + How often tick() is called is the control loop's business: the rate is a + session knob living in ControllerState, and a copy here would be a second + answer to it. """ def __init__( @@ -40,7 +43,6 @@ def __init__( port: int = cfg.MCAST_PORT, ttl: int = cfg.MCAST_TTL, iface_ip: str = cfg.MCAST_IF, - rate_hz: float = cfg.STATUS_RATE_HZ, stale_s: float = cfg.STATUS_STALE_S, ) -> None: self._state_mgr = state_mgr diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index d6c666f..4babe2a 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -39,6 +39,16 @@ from parol6.tools import get_tool_transform from parol6 import config as _cfg +# Drive-fault labels indexed by (overtemperature | following-error << 1). +# Built once: the bits are read every control tick, and the repo's hot path +# does not allocate. +_DRIVE_FAULT_LABELS: tuple[tuple[str, ...], ...] = ( + (), + ("overtemperature",), + ("following_error",), + ("overtemperature", "following_error"), +) + logger = logging.getLogger(__name__) @@ -198,16 +208,19 @@ def __init__(self) -> None: self._tcp_pos_buf: np.ndarray = np.zeros(3, dtype=np.float64) self._tcp_pos_initialized: bool = False - self._status_rate_hz: float = _cfg.STATUS_RATE_HZ + # Broadcast period the last TCP sample was taken at. The rate is a + # session knob, and the gap being differentiated was governed by the + # period in force when the earlier sample was taken, not by the one + # that has just replaced it. + self._tcp_sample_period_s: float = ( + _cfg.INTERVAL_S * _cfg.status_broadcast_interval(_cfg.STATUS_RATE_HZ) + ) - # Per-joint drive faults. The firmware reports two bitfields; the - # label view is rebuilt only when a bit moves, so the 100 Hz path - # allocates nothing while the drives are healthy. One entry per - # joint always — an all-clear list of empty tuples is how a consumer - # tells "this backend reports faults, none active" from "this - # backend has no fault reporting", which is an empty list. - self._temp_fault_bits = np.zeros(6, dtype=np.uint8) - self._poserr_fault_bits = np.zeros(6, dtype=np.uint8) + # Per-joint drive faults, one bit per condition. One entry per joint + # always — an all-clear list of empty tuples is how a consumer tells + # "this backend reports faults, none active" from "this backend has + # no fault reporting", which is an empty list. + self._drive_fault_bits = np.zeros(6, dtype=np.uint8) self._drive_faults: list[tuple[str, ...]] = [() for _ in range(6)] # IK enablement results (pre-allocated for zero-alloc reads) @@ -450,7 +463,7 @@ def update_from_state(self, state: ControllerState) -> None: self._tcp_pos_buf[1] = self.pose[7] self._tcp_pos_buf[2] = self.pose[11] if self._tcp_pos_initialized: - dt = 1.0 / self._status_rate_hz + dt = self._tcp_sample_period_s dx = self._tcp_pos_buf[0] - self._prev_tcp_pos[0] dy = self._tcp_pos_buf[1] - self._prev_tcp_pos[1] dz = self._tcp_pos_buf[2] - self._prev_tcp_pos[2] @@ -458,6 +471,9 @@ def update_from_state(self, state: ControllerState) -> None: else: self._tcp_pos_initialized = True self._prev_tcp_pos[:] = self._tcp_pos_buf + self._tcp_sample_period_s = ( + _cfg.INTERVAL_S * _cfg.status_broadcast_interval(state.status_rate_hz) + ) else: # Robot not moving — reset TCP speed to zero self.tcp_speed = 0.0 @@ -541,22 +557,13 @@ def update_from_state(self, state: ControllerState) -> None: faults_changed = False for i in range(6): - temp_bit = 1 if state.Temperature_error_in[i] else 0 - pos_bit = 1 if state.Position_error_in[i] else 0 - if self._temp_fault_bits[i] != temp_bit: - self._temp_fault_bits[i] = temp_bit - faults_changed = True - if self._poserr_fault_bits[i] != pos_bit: - self._poserr_fault_bits[i] = pos_bit + bits = (1 if state.Temperature_error_in[i] else 0) | ( + 2 if state.Position_error_in[i] else 0 + ) + if self._drive_fault_bits[i] != bits: + self._drive_fault_bits[i] = bits + self._drive_faults[i] = _DRIVE_FAULT_LABELS[bits] faults_changed = True - if faults_changed: - for i in range(6): - labels = [] - if self._temp_fault_bits[i]: - labels.append("overtemperature") - if self._poserr_fault_bits[i]: - labels.append("following_error") - self._drive_faults[i] = tuple(labels) # Only a live HomeCommand owns homing_step; any cancel path that drops # the command clears action_current, so derive "idle" from that. @@ -653,18 +660,6 @@ def to_binary(self) -> bytes: self._binary_dirty = False return self._binary_cache - def set_status_rate(self, hz: float) -> None: - """Adopt a new broadcast rate. - - The cache differentiates TCP position against the broadcast period, - so a rate change that missed this would scale every reported speed. - """ - self._status_rate_hz = hz - - @property - def status_rate_hz(self) -> float: - return self._status_rate_hz - def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" self.last_serial_s = time.monotonic() diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index d9f732a..4a75378 100644 --- a/parol6/utils/error_catalog.py +++ b/parol6/utils/error_catalog.py @@ -164,15 +164,21 @@ class _ErrorTemplate: def make_error( code: ErrorCode, command_index: int = -1, **params: object ) -> RobotError: - """Create a RobotError from the catalog, formatting placeholders in title/cause.""" + """Create a RobotError from the catalog, formatting its placeholders. + + Every field is formatted, not just the ones that usually carry a + placeholder: a remedy is where a refusal says what would have worked, so + a template that puts its parameter there must not reach the client with + the placeholder still in it. + """ tmpl = _CATALOG[code] return RobotError( command_index=command_index, code=int(code), title=tmpl.title.format_map(params) if params else tmpl.title, cause=tmpl.cause.format_map(params) if params else tmpl.cause, - effect=tmpl.effect, - remedy=tmpl.remedy, + effect=tmpl.effect.format_map(params) if params else tmpl.effect, + remedy=tmpl.remedy.format_map(params) if params else tmpl.remedy, ) diff --git a/parol6/utils/errors.py b/parol6/utils/errors.py index f2bfa19..a47d793 100644 --- a/parol6/utils/errors.py +++ b/parol6/utils/errors.py @@ -29,14 +29,6 @@ def __init__(self, robot_error: RobotError): super().__init__(str(robot_error)) -class ConfigurationError(RuntimeError): - """A configuration command the controller cannot honour.""" - - def __init__(self, robot_error: RobotError): - self.robot_error = robot_error - super().__init__(str(robot_error)) - - class MotionError(RuntimeError): """Pipeline planning/execution error detected via status broadcast.""" diff --git a/tests/integration/test_status_broadcast_autofailover.py b/tests/integration/test_status_broadcast_autofailover.py index 4e333af..fa603d9 100644 --- a/tests/integration/test_status_broadcast_autofailover.py +++ b/tests/integration/test_status_broadcast_autofailover.py @@ -49,7 +49,6 @@ def _force_unicast_setup(self: StatusBroadcaster) -> None: group=group, port=port, iface_ip=iface, - rate_hz=20.0, stale_s=1.0, ) @@ -155,7 +154,7 @@ async def test_multicast_send_errors_should_trigger_fallback_but_currently_do_no state_mgr = StateManager() broadcaster = StatusBroadcaster( - state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", rate_hz=20.0, stale_s=2.0 + state_mgr=state_mgr, port=port, iface_ip="127.0.0.1", stale_s=2.0 ) # StatusBroadcaster is now a polling class - call tick() manually diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py index 8e58fee..798786e 100644 --- a/tests/integration/test_status_rate.py +++ b/tests/integration/test_status_rate.py @@ -13,6 +13,10 @@ import pytest from parol6 import AsyncRobotClient +from parol6.server.state import ControllerState +from parol6.server.status_cache import StatusCache +from parol6.utils.error_codes import ErrorCode +from parol6.utils.errors import MotionError async def _observed_hz(client: AsyncRobotClient, frames: int = 40) -> float: @@ -88,24 +92,77 @@ async def test_raising_the_rate_delivers_more_frames(server_proc, ports): @pytest.mark.asyncio @pytest.mark.integration async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports): - """Refused, never rounded to a neighbour: a capture taken at a rate - nobody asked for is wrong in a way nothing reports. The refusal has to - carry what the legal rates are, since that is all the operator needs.""" + """Refused, never rounded to a neighbour: a capture taken at a rate nobody + asked for is wrong in a way nothing reports. The refusal has to reach the + caller carrying the rates that would have worked, since that is the whole + of what an operator needs — including for the rates whose arithmetic the + check itself cannot survive: 0.5 Hz floors to a zero divisor, and NaN + cannot be made an int at all, so a validator that divides before it + screens turns a refusal into a crash. + """ async with AsyncRobotClient(port=ports.server_port) as client: assert await client.wait_ready(timeout=10.0) before = await client.status_rate() assert before is not None + achievable = before.achievable() - bogus = before.control_hz / 3 + 0.5 - assert bogus not in before.achievable() + for bogus in (0.0, -50.0, 0.5, 62.5, float("nan"), float("inf")): + assert bogus not in achievable + with pytest.raises(MotionError) as caught: + await client.set_status_rate(bogus) - with pytest.raises(Exception) as caught: - await client.set_status_rate(bogus) - message = str(caught.value) - assert str(int(before.control_hz)) in message, message - assert "divide" in message.lower(), message + refusal = caught.value.robot_error + assert refusal.code == ErrorCode.SYS_STATUS_RATE_INVALID, ( + f"{bogus} Hz came back as {refusal.title!r} rather than as an " + f"unservable rate: {refusal.cause}" + ) + unnamed = [hz for hz in achievable if str(int(hz)) not in refusal.remedy] + assert not unnamed, ( + f"refusing {bogus} Hz has to say what would work instead, but " + f"{unnamed} are missing from {refusal.remedy!r}" + ) after = await client.status_rate() assert after is not None and after.hz == before.hz, ( "a refused rate must leave the broadcast alone" ) + + +@pytest.mark.integration +def test_the_speed_derivative_follows_the_rate_it_was_sampled_at(): + """TCP speed is a difference over the broadcast period, so the period the + cache divides by has to be the one the controller is actually broadcasting + at — and the sample that straddles a rate change spans the period it was + taken at, not the one that has just replaced it. Get either wrong and a + steady arm appears to change speed the moment somebody changes the rate. + + Only J1 moves, so equal step increments are equal chords of one circle + about the base axis: the displacement is the same every sample, and any + change in the reported speed is the period alone. + """ + cache = StatusCache() + try: + state = ControllerState() + + def advance() -> float: + state.Position_in[0] += 200 + cache.update_from_state(state) + return cache.tcp_speed + + advance() # first difference has nothing to difference against + at_50 = advance() + assert at_50 > 0.0, "a moving arm has to report a speed" + + state.status_rate_hz = 25.0 + straddling = advance() + settled = advance() + + assert straddling == pytest.approx(at_50, rel=1e-3), ( + "the sample taken before the rate changed spans the old period" + ) + assert settled == pytest.approx(at_50 / 2, rel=1e-3), ( + "half the broadcast rate is twice the period, so the same " + f"movement per frame is half the speed: {settled} vs {at_50}" + ) + finally: + cache.close() From 3c01028c9672c041b9beec47843f70af14a82a37 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 6 Sep 2026 21:15:06 -0400 Subject: [PATCH 3/6] Add sync skill execution and report stopped planned motion accurately --- .github/workflows/tests.yml | 28 +++++------ parol6/client/async_client.py | 4 ++ parol6/client/dry_run_client.py | 4 ++ parol6/client/sync_client.py | 10 ++++ parol6/server/segment_player.py | 6 +++ pyproject.toml | 2 +- tests/integration/test_skills.py | 82 ++++++++++++++++++++++++++++++++ 7 files changed, 121 insertions(+), 15 deletions(-) create mode 100644 tests/integration/test_skills.py diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml index 8bba775..6992620 100644 --- a/.github/workflows/tests.yml +++ b/.github/workflows/tests.yml @@ -27,15 +27,15 @@ jobs: # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0; # pre-install the fixed commit until a release lands (pantor/ruckig#262). pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff" - pip install -e ".[dev]" - # Override the pinned waldoctl with the matching feature branch if one - # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag - # in pyproject can't clobber it. Deps are kept (no --no-deps): the - # refactored waldoctl imports nicegui, which parol6 doesn't otherwise - # install. Skipped on main so main CI exercises the released pin. + # Resolve the shared contract branch before the package: the new + # release tag is created only after its companion PR merges. BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}" if [ "$BRANCH" != "main" ] && git ls-remote --heads https://github.com/Jepson2k/waldoctl.git "$BRANCH" 2>/dev/null | grep -q .; then - pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}" + sed -i.bak "s#waldoctl.git@v[0-9.]*#waldoctl.git@${BRANCH}#" pyproject.toml + fi + pip install -e ".[dev]" + if [ -f pyproject.toml.bak ]; then + mv pyproject.toml.bak pyproject.toml fi - name: Run pre-commit uses: pre-commit/action@v3.0.1 @@ -112,15 +112,15 @@ jobs: # ruckig 0.17.3 sdist doesn't build under scikit-build-core 1.0; # pre-install the fixed commit until a release lands (pantor/ruckig#262). pip install "ruckig @ git+https://github.com/pantor/ruckig@2249d57ffaa19ecdadeaab62daf97857813629ff" - pip install -e ".[dev]" pytest-timeout - # Override the pinned waldoctl with the matching feature branch if one - # exists, AFTER ".[dev]" (and with --force-reinstall) so the pinned tag - # in pyproject can't clobber it. Deps are kept (no --no-deps): the - # refactored waldoctl imports nicegui, which parol6 doesn't otherwise - # install. Skipped on main so main CI exercises the released pin. + # Resolve the shared contract branch before the package: the new + # release tag is created only after its companion PR merges. BRANCH="${GITHUB_HEAD_REF:-${GITHUB_REF_NAME}}" if [ "$BRANCH" != "main" ] && git ls-remote --heads https://github.com/Jepson2k/waldoctl.git "$BRANCH" 2>/dev/null | grep -q .; then - pip install --force-reinstall "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@${BRANCH}" + sed -i.bak "s#waldoctl.git@v[0-9.]*#waldoctl.git@${BRANCH}#" pyproject.toml + fi + pip install -e ".[dev]" pytest-timeout + if [ -f pyproject.toml.bak ]; then + mv pyproject.toml.bak pyproject.toml fi # Override the pinned pinokin v0.1.6 wheel with the matching-branch diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 43c8953..c23a208 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -243,6 +243,10 @@ class AsyncRobotClient(_RobotClientABC): Query commands: request/response with timeout and simple retry """ + @property + def skill_capabilities(self) -> frozenset[str]: + return super().skill_capabilities | {"backend.parol6"} + def __init__( self, host: str = "127.0.0.1", diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 8b04a49..d6d34fd 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -520,6 +520,10 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: # ---- Explicit methods for state reads ---- + @property + def skill_capabilities(self) -> frozenset[str]: + return frozenset({"motion.joint", "motion.linear", "backend.parol6"}) + def angles(self) -> list[float]: steps_to_rad(self._state.Position_in, self._q_rad_buf) return np.degrees(self._q_rad_buf).tolist() diff --git a/parol6/client/sync_client.py b/parol6/client/sync_client.py index 803edd4..ff1d217 100644 --- a/parol6/client/sync_client.py +++ b/parol6/client/sync_client.py @@ -145,6 +145,16 @@ def _bind_default_tools(self) -> None: # ---------- tool access ---------- + def run_skill( + self, invoke: Callable[[AsyncRobotClient], Coroutine[Any, Any, T]] + ) -> T: + """Execute a Python skill using this connection and its existing loop.""" + return _run(invoke(self._inner)) + + @property + def skill_capabilities(self) -> frozenset[str]: + return self._inner.skill_capabilities + @property def tool(self) -> SyncTool: """Active bound tool. Raises if no tool has been set.""" diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 080e2a2..29d686b 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -343,6 +343,12 @@ def _world_guard( def cancel(self, state: ControllerState) -> None: """Clear buffer, drain stale segments, and stop playback.""" + if self._active is not None: + # Planned trajectories live here rather than in CommandExecutor. + # Cancelling its command cannot clear this player's activity. + state.action_current = "" + state.action_params = "" + state.action_state = ActionState.IDLE self._active = None self._step = 0 self._inline_cmd = None diff --git a/pyproject.toml b/pyproject.toml index 692601c..dcc440e 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.12.0", + "waldoctl @ git+https://github.com/Jepson2k/waldoctl.git@v0.14.0", ] [tool.setuptools.packages.find] diff --git a/tests/integration/test_skills.py b/tests/integration/test_skills.py new file mode 100644 index 0000000..4870254 --- /dev/null +++ b/tests/integration/test_skills.py @@ -0,0 +1,82 @@ +"""The generic skill contract over the real fake-serial controller.""" + +import asyncio + +import pytest +from waldoctl.skills import observe_skills, skill + +from parol6 import AsyncRobotClient +from parol6.protocol.wire import ActionState + + +@skill( + id="test.nudge", + version="1.0.0", + requires=frozenset({"motion.joint", "backend.parol6"}), +) +async def nudge(rbt: AsyncRobotClient, *, degrees: float) -> list[float]: + target = await rbt.angles() + assert target is not None + target[0] += degrees + index = await rbt.move_j(target, speed=0.5) + assert index >= 0 and await rbt.wait_command(index, timeout=20.0) + observed = await rbt.angles() + assert observed is not None + return observed + + +def test_sync_skill_uses_the_supplied_connection_and_preserves_results( + client, server_proc +): + before = client.angles() + events = [] + with observe_skills(events.append): + after = nudge(client, degrees=-5.0) + assert after[0] == pytest.approx(before[0] - 5.0, abs=0.5) + assert [event.phase for event in events] == ["started", "completed"] + + +def test_async_cancel_stops_the_controller_and_prevents_the_next_move( + client, server_proc, ports +): + async def scenario(): + async with AsyncRobotClient( + host=ports.server_ip, port=ports.server_port + ) as rbt: + start = await rbt.angles() + assert start is not None + moving = asyncio.Event() + + @skill(id="test.cancel", version="1.0.0") + async def sequence(rbt: AsyncRobotClient) -> None: + target = list(start) + target[0] -= 20 + index = await rbt.move_j(target, duration=5.0) + assert index >= 0 + moving.set() + try: + await rbt.wait_command(index, timeout=15.0) + except asyncio.CancelledError: + # Catching cancellation must not allow another command. + await rbt.move_j(start, speed=0.5) + pytest.fail("a cancelled invocation issued another command") + + events = [] + with observe_skills(events.append): + task = asyncio.create_task(sequence.async_call(rbt)) + await asyncio.wait_for(moving.wait(), timeout=10.0) + assert await rbt.wait_status( + lambda s: s.angles[0] < start[0] - 1.0, timeout=10.0 + ) + task.cancel() + with pytest.raises(asyncio.CancelledError): + await task + assert events[-1].phase == "cancelled" + assert events[-1].stop_confirmed is True + assert await rbt.wait_status( + lambda s: s.action_state == ActionState.IDLE and s.queued_segments == 0, + timeout=3.0, + ), await rbt.activity() + assert await nudge.async_call(rbt, degrees=-2.0) is not None + + asyncio.run(scenario()) From 4822cbabbcb4e465cf8659a9787ad4cd034d9bb2 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Sun, 6 Sep 2026 22:12:41 -0400 Subject: [PATCH 4/6] test: choose a bindable status port for example subprocesses --- tests/test_examples.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_examples.py b/tests/test_examples.py index b4a5d77..b36fd11 100644 --- a/tests/test_examples.py +++ b/tests/test_examples.py @@ -27,11 +27,13 @@ @pytest.mark.examples @pytest.mark.timeout(300) @pytest.mark.parametrize("script", EXAMPLES) -def test_example_runs(script): +def test_example_runs(script, ports): """Run each example as a subprocess and check it exits cleanly.""" result = subprocess.run( [sys.executable, str(EXAMPLES_DIR / script)], - env=ENV, + # Windows can reserve the default status port even with no listener. + # The subprocess and its controller share the OS-probed test port. + env={**ENV, "PAROL6_MCAST_PORT": str(ports.mcast_port)}, capture_output=True, text=True, timeout=240, From a8a8aef9b5c4f5dccf1e09524617eb5b8de25c3e Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 20:59:31 +0000 Subject: [PATCH 5/6] Answer a state command with its code in the dry run, and take the live jog arguments Two ways a program could pass preview and fail on the arm, or the reverse. A system or control command returned the planner result object, so a program branching on the code compared an int with a result. It answers the live client's 1/0/negative now, read from the command table rather than a second list of names. jog_j and jog_l were auto-dispatched straight into the wire struct, whose fields are six-element speed vectors -- so the documented call, rbt.jog_j(0, 0.5, 1.0), previewed as a refusal about a subscript. They take the live client's single-joint and multi-joint signatures now, mapping to the vector the same way it does. Co-Authored-By: Claude Opus 5 --- parol6/client/dry_run_client.py | 67 ++++++++++++++++++++++-- tests/unit/test_dry_run_script_compat.py | 57 +++++++++++++++++++- 2 files changed, 120 insertions(+), 4 deletions(-) diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index d6d34fd..780bec6 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -38,6 +38,7 @@ import re as _re import parol6.protocol.wire as _wire +from waldoctl.commands import CommandKind, command_table from ..protocol.wire import ( HomeCmd, SelectToolCmd, @@ -77,6 +78,9 @@ def _pascal_to_snake(name: str) -> str: _UPPER_FIELDS: frozenset[str] = frozenset({"tool_name", "tool_key", "profile"}) +_COMMANDS = command_table() +_AXIS_INDEX: dict[str, int] = {"X": 0, "Y": 1, "Z": 2, "RX": 3, "RY": 4, "RZ": 5} + def build_cmd(name: str, *args: Any, **kwargs: Any) -> Any: """Build a command struct by method name.""" @@ -576,6 +580,52 @@ def servo_j( return self._dispatch(build_cmd("servo_j_pose", pose, **kwargs)) return self._dispatch(build_cmd("servo_j", angles or [], **kwargs)) + def jog_j( + self, + joint: int = -1, + speed: float = 0.0, + duration: float = 0.1, + *, + joints: list[int] | None = None, + speeds: list[float] | None = None, + accel: float = 1.0, + ) -> DryRunResult | None: + """The live client's signature, so a script's jog previews as written.""" + speed_arr = [0.0] * 6 + if joints is not None and speeds is not None: + for j, s in zip(joints, speeds): + speed_arr[j] = s + elif joint >= 0: + speed_arr[joint] = speed + else: + raise ValueError("jog_j requires either joint= or joints=/speeds=") + return self._dispatch( + _wire.JogJCmd(speeds=speed_arr, duration=duration, accel=accel) + ) + + def jog_l( + self, + frame: str, + axis: str | None = None, + speed: float = 0.0, + duration: float = 0.1, + *, + axes: list[str] | None = None, + speeds_list: list[float] | None = None, + accel: float = 1.0, + ) -> DryRunResult | None: + vel = [0.0] * 6 + if axes is not None and speeds_list is not None: + for a, s in zip(axes, speeds_list): + vel[_AXIS_INDEX[a]] = s + elif axis is not None: + vel[_AXIS_INDEX[axis]] = speed + else: + raise ValueError("jog_l requires either axis= or axes=/speeds_list=") + return self._dispatch( + _wire.JogLCmd(frame=frame, velocities=vel, duration=duration, accel=accel) + ) + def delay(self, seconds: float = 0.0) -> None: pass @@ -590,8 +640,19 @@ def __getattr__(self, name: str) -> Any: if name not in _CMD_STRUCTS: raise AttributeError(f"'{type(self).__name__}' has no attribute '{name}'") - def method(*args: Any, **kwargs: Any) -> DryRunResult | None: - cmd = build_cmd(name, *args, **kwargs) - return self._dispatch(cmd) + spec = _COMMANDS.get(name) + applies = spec is not None and spec.kind in ( + CommandKind.SYSTEM, + CommandKind.CONTROL, + ) + + def method(*args: Any, **kwargs: Any) -> DryRunResult | int | None: + result = self._dispatch(build_cmd(name, *args, **kwargs)) + if not applies: + return result + # A system or control command answers as the live client does: + # 1 when it applied, negative when the planner refused it. Its + # planner result carries no path a program could wait on. + return -1 if result is not None and result.error is not None else 1 return method diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py index b6e89e8..44a6c8f 100644 --- a/tests/unit/test_dry_run_script_compat.py +++ b/tests/unit/test_dry_run_script_compat.py @@ -11,8 +11,9 @@ import numpy as np import pytest +from waldoctl import CommandKind, command_table -from parol6.client.dry_run_client import DryRunRobotClient +from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] POSE_A = [0.0, 280.0, 200.0, 90.0, 0.0, 90.0] @@ -176,3 +177,57 @@ def test_snap_carries_the_pending_blend_chain(self): assert len(result.joint_trajectory_rad) > 1 assert np.allclose(np.degrees(result.end_joints_rad), HOME, atol=0.5) assert client.flush() == [] + + +def test_jogs_take_the_live_clients_arguments(client): + """`rbt.jog_j(0, 0.5, 1.0)` and `rbt.jog_l("WRF", "X", 0.5, 1.0)` are the + forms the docs show; the preview must plan them, not the wire struct's + field order.""" + before = np.asarray(client.angles()) + result = client.jog_j(0, 0.5, 1.0) + assert result is not None and result.error is None + after = np.degrees(result.end_joints_rad) + assert after[0] > before[0] + 1.0 + assert np.allclose(after[1:], before[1:], atol=1e-6) + + client = DryRunRobotClient() + x_before = client.pose()[0] + result = client.jog_l("WRF", "X", 0.5, 1.0) + assert result is not None and result.error is None + assert result.tcp_poses[-1][0] * 1000.0 > x_before + 1.0 + assert client.pose()[0] > x_before + 1.0 + with pytest.raises(ValueError, match="joint="): + client.jog_j(speed=0.5) + + +_STATE_ARGS = { + "reset": (), + "reset_state": (), + "set_status_rate": (50,), + "simulator": (True,), + "teleport": (HOME,), + "set_shapes": ([],), + "select_profile": ("RUCKIG",), + "select_tool": ("NONE",), + "set_tcp_offset": (0.0, 0.0, 0.0), + "connect_hardware": ("/dev/null",), + "stop": (), + "estop": (), +} + + +@pytest.mark.parametrize( + "name", + sorted( + n + for n, s in command_table().items() + if s.kind in (CommandKind.SYSTEM, CommandKind.CONTROL) and n in _CMD_STRUCTS + ), +) +def test_state_commands_answer_with_the_live_clients_int_codes(client, name): + """`if rbt.stop() < 0:` must read the same in preview as on the arm: a + system or control command returns 1/0/negative, never a planner result.""" + assert name in _STATE_ARGS, f"add sample arguments for {name}" + result = getattr(client, name)(*_STATE_ARGS[name]) + assert isinstance(result, int) and not isinstance(result, bool) + assert result == 1 From a1d334934361848048560591b22101c7f80d43ce Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 21:28:02 +0000 Subject: [PATCH 6/6] Report the rates the controller serves, and its exact control rate The controller knows its divisor set well enough to format it into the refusal, but the status-rate query did not report it, so every consumer fell back to re-deriving one backend's every-Nth-tick rule -- the guess waldoctl's contract warns against, and one that offers nothing at all for a non-integer loop rate. The query now answers with the set, from the same helper the refusal names it with. control_hz came from inverting INTERVAL_S, which is itself 1/rate: at a 49 Hz loop that reports 49.00000000000001 and the exact-divisor assertions it feeds stop holding. It reports the configured rate. The speed-derivative test hard-coded the 50 Hz default while the cache reads the rate from the state, so it failed in a shell that configured another rate. It halves whatever rate is configured. Co-Authored-By: Claude Opus 5 --- parol6/client/async_client.py | 6 +++++- parol6/commands/query_commands.py | 6 +++++- parol6/commands/utility_commands.py | 6 ++---- parol6/config.py | 13 +++++++++++++ parol6/protocol/wire.py | 5 ++++- tests/integration/test_status_rate.py | 28 +++++++++++++++++++-------- 6 files changed, 49 insertions(+), 15 deletions(-) diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index ad6d339..13886f4 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -965,7 +965,11 @@ async def status_rate(self) -> StatusRate | None: resp = await self._request(StatusRateCmd()) if not isinstance(resp, StatusRateResultStruct): return None - return StatusRate(hz=resp.hz, control_hz=resp.control_hz) + return StatusRate( + hz=resp.hz, + control_hz=resp.control_hz, + servable=tuple(float(v) for v in resp.servable), + ) async def select_tool(self, tool_name: str, variant_key: str = "") -> int: """Set the active end-effector tool on the controller. diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 0f092a2..a2a69e0 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -197,7 +197,11 @@ 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), + # 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(), ) ) diff --git a/parol6/commands/utility_commands.py b/parol6/commands/utility_commands.py index 6ec7821..5180eb2 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,7 +11,7 @@ MotionCommand, SystemCommand, ) -from parol6.config import CONTROL_RATE_HZ +from parol6.config import CONTROL_RATE_HZ, servable_status_rates from parol6.protocol.wire import ( CheckpointCmd, CmdType, @@ -115,9 +115,7 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode: # as a generic tick failure instead of the refusal that names the # rates this controller can serve. if not (1.0 <= hz <= control) or not hz.is_integer() or control % int(hz) != 0: - allowed = ", ".join( - str(control // n) for n in range(1, control + 1) if control % n == 0 - ) + allowed = ", ".join(f"{hz:g}" for hz in servable_status_rates()) self.fail( make_error( ErrorCode.SYS_STATUS_RATE_INVALID, diff --git a/parol6/config.py b/parol6/config.py index f3f3046..b738120 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -99,6 +99,19 @@ def status_broadcast_interval(hz: float) -> int: return max(1, int(CONTROL_RATE_HZ) // int(hz)) +def servable_status_rates() -> tuple[float, ...]: + """Broadcast rates this controller accepts, highest first. + + Status goes out every Nth control tick, so the servable rates are the + divisors of the control rate. One answer, used by the query that reports + the set and by the refusal that names it. + """ + control = int(CONTROL_RATE_HZ) + return tuple( + float(control // n) for n in range(1, control + 1) if control % n == 0 + ) + + # Validate STATUS_RATE_HZ divides evenly into CONTROL_RATE_HZ for polling if int(CONTROL_RATE_HZ) % int(STATUS_RATE_HZ) != 0: raise ValueError( diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 57082a7..5e5b7a0 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -1040,10 +1040,13 @@ class StatusRateResultStruct( frozen=True, gc=False, ): - """Broadcast rate, and the control rate it divides.""" + """Broadcast rate, the control rate it divides, and the rates the + controller accepts -- its own answer, so a caller can pick one that will + be accepted instead of discovering the constraint by rejection.""" hz: float control_hz: float + servable: tuple[float, ...] = () class LoopStatsResultStruct( diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py index 798786e..dac25f3 100644 --- a/tests/integration/test_status_rate.py +++ b/tests/integration/test_status_rate.py @@ -104,6 +104,14 @@ async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports) assert await client.wait_ready(timeout=10.0) before = await client.status_rate() assert before is not None + assert before.servable, ( + "the controller knows its divisor set -- it formats it into the " + "refusal -- so the query has to report it rather than leaving the " + "client to re-derive one backend's rule" + ) + assert before.achievable() == before.servable + assert before.hz in before.servable + assert before.control_hz == max(before.servable) achievable = before.achievable() for bogus in (0.0, -50.0, 0.5, 62.5, float("nan"), float("inf")): @@ -116,7 +124,7 @@ async def test_an_unachievable_rate_is_refused_with_the_rule(server_proc, ports) f"{bogus} Hz came back as {refusal.title!r} rather than as an " f"unservable rate: {refusal.cause}" ) - unnamed = [hz for hz in achievable if str(int(hz)) not in refusal.remedy] + unnamed = [hz for hz in achievable if f"{hz:g}" not in refusal.remedy] assert not unnamed, ( f"refusing {bogus} Hz has to say what would work instead, but " f"{unnamed} are missing from {refusal.remedy!r}" @@ -150,19 +158,23 @@ def advance() -> float: return cache.tcp_speed advance() # first difference has nothing to difference against - at_50 = advance() - assert at_50 > 0.0, "a moving arm has to report a speed" - - state.status_rate_hz = 25.0 + started = advance() + assert started > 0.0, "a moving arm has to report a speed" + + # Halve whatever rate this environment configured, rather than + # assuming the 50 Hz default: the cache reads its period from the + # state, so a shell with PAROL6_STATUS_RATE_HZ set would otherwise + # fail the ratio for reasons that have nothing to do with the code. + state.status_rate_hz = state.status_rate_hz / 2 straddling = advance() settled = advance() - assert straddling == pytest.approx(at_50, rel=1e-3), ( + assert straddling == pytest.approx(started, rel=1e-3), ( "the sample taken before the rate changed spans the old period" ) - assert settled == pytest.approx(at_50 / 2, rel=1e-3), ( + assert settled == pytest.approx(started / 2, rel=1e-3), ( "half the broadcast rate is twice the period, so the same " - f"movement per frame is half the speed: {settled} vs {at_50}" + f"movement per frame is half the speed: {settled} vs {started}" ) finally: cache.close()