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..6ec7821 100644 --- a/parol6/commands/utility_commands.py +++ b/parol6/commands/utility_commands.py @@ -11,13 +11,17 @@ MotionCommand, SystemCommand, ) +from parol6.config import CONTROL_RATE_HZ from parol6.protocol.wire import ( CheckpointCmd, CmdType, DelayCmd, ResetLoopStatsCmd, ResetStateCmd, + SetStatusRateCmd, ) +from parol6.utils.error_catalog import make_error +from parol6.utils.error_codes import ErrorCode from parol6.protocol.wire import CommandCode from parol6.server.command_registry import register_command from parol6.server.state import ControllerState @@ -89,6 +93,46 @@ 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) + # 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 + ) + self.fail( + make_error( + ErrorCode.SYS_STATUS_RATE_INVALID, + requested=hz, + control=control, + allowed=allowed, + ) + ) + return ExecutionStatusCode.FAILED + state.status_rate_hz = 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/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/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..25d06b0 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -71,9 +71,8 @@ MCAST_PORT, MCAST_IF, MCAST_TTL, - STATUS_RATE_HZ, STATUS_STALE_S, - STATUS_BROADCAST_INTERVAL, + status_broadcast_interval, ) import psutil @@ -193,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, @@ -201,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") @@ -538,7 +536,12 @@ def _main_control_loop(self): self._timer.start() pt = self._phase_timer tick_count = 0 - broadcast_interval = STATUS_BROADCAST_INTERVAL + # 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 = 1 while self.running: try: @@ -558,6 +561,10 @@ 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 = status_broadcast_interval(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_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 9cb270b..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,7 +208,20 @@ 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, 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) self._joint_en = np.ones(12, dtype=np.uint8) @@ -440,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] @@ -448,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 @@ -529,6 +555,16 @@ def update_from_state(self, state: ControllerState) -> None: if enabled_changed: self._enabled = state.enabled + faults_changed = False + for i in range(6): + 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 + # 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 +616,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,6 +655,7 @@ 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 diff --git a/parol6/utils/error_catalog.py b/parol6/utils/error_catalog.py index 396d3cd..4a75378 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}", @@ -157,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/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/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_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 new file mode 100644 index 0000000..798786e --- /dev/null +++ b/tests/integration/test_status_rate.py @@ -0,0 +1,168 @@ +"""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 +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: + """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 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() + + 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) + + 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() 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