diff --git a/README.md b/README.md index c4136e3..5ca62de 100644 --- a/README.md +++ b/README.md @@ -336,6 +336,20 @@ case. A timed-out wait leaves the motion queued; `stop()` cancels it. Planning preview retimes trajectories and reports paused queued operations as `UnresolvedPreview` instead of claiming completion. +## Timed observations + +`stream_status()` supplies `session_id`, `seq` and `mono_time_ns` for recording +observations. The session identifies the status publisher's lifetime and changes +on restart. Sequence gaps reveal missed publications; the monotonic timestamp +marks publication of the current controller snapshot, not simultaneous sensor +acquisition. Status without these fields reports zero metadata and cannot support +identified demonstration capture. The client advertises `observation.timed`. + +Waldo Commander's `record_demonstration` stores this metadata and its host receipt +time with the observed joints and tool state. Its replay skill uses ordinary +native joint moves/delays, including native retiming, completion and collision +checks; no continuous recorded-trajectory command is added. + ## Command system Jog and servo commands (JogJ, JogL, ServoJ, ServoL) automatically use the streaming fast-path — the server de-duplicates stale inputs, reduces ACK chatter, and reuses the active command. Use jog/servo for UI-driven motion or teleoperation; use planned moves (MoveJ, MoveL, etc.) for discrete motions and queued programs. diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index fa931bc..e155c91 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -244,7 +244,7 @@ def __init__( self._q_rad_buf = np.zeros(6, dtype=np.float64) self._rpy_buf = np.zeros(3, dtype=np.float64) self._max_snapshot_points = max_snapshot_points - self._active_tool_key: str = "" + self._active_tool_key: str = "NONE" self._active_variant_key: str = "" self._tool_proxy = _DryRunTool(self) diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index d026091..d9d3edc 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -1547,6 +1547,9 @@ def pack_status( p99_period_s: float = 0.0, overruns: int = 0, drive_faults: Sequence[Sequence[str]] = (), + session_id: int = 0, + seq: int = 0, + mono_time_ns: int = 0, ) -> bytes: """Pack a status broadcast message. @@ -1598,6 +1601,9 @@ def pack_status( joints_homed, (p99_period_s, overruns), drive_faults, + session_id, + seq, + mono_time_ns, ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1616,6 +1622,9 @@ class StatusBuffer: Use decode_status_bin_into() to fill this buffer without allocating new objects. """ + session_id: int = 0 + seq: int = 0 + mono_time_ns: int = 0 pose: np.ndarray = field(default_factory=lambda: np.zeros(16, dtype=np.float64)) angles: np.ndarray = field(default_factory=lambda: np.zeros(6, dtype=np.float64)) speeds: np.ndarray = field(default_factory=lambda: np.zeros(6, dtype=np.float64)) @@ -1697,6 +1706,9 @@ def copy(self) -> "StatusBuffer": """Return a deep copy with all arrays copied.""" ts = self.tool_status return StatusBuffer( + session_id=self.session_id, + seq=self.seq, + mono_time_ns=self.mono_time_ns, pose=self.pose.copy(), angles=self.angles.copy(), speeds=self.speeds.copy(), @@ -1784,7 +1796,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, drive_faults] + loop_health, drive_faults, session_id, seq, mono_time_ns] Args: data: Raw msgpack bytes @@ -1807,6 +1819,19 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: if msg[1] != PROTO_VERSION: raise ProtocolVersionError(msg[1]) + # session_id / seq / mono_time_ns ride at the end, one slot later now + # that the protocol version leads the message. + if 31 < len(msg) < 34: + return False + if len(msg) >= 34: + for index in range(31, 34): + value = msg[index] + if type(value) is not int or not 0 <= value <= 0xFFFFFFFFFFFFFFFF: + return False + buf.session_id, buf.seq, buf.mono_time_ns = msg[31], msg[32], msg[33] + else: + buf.session_id = buf.seq = buf.mono_time_ns = 0 + buf.pose[:] = msg[2] buf.angles[:] = msg[3] buf.speeds[:] = msg[4] diff --git a/parol6/server/status_broadcast.py b/parol6/server/status_broadcast.py index 9754ba6..fb800d5 100644 --- a/parol6/server/status_broadcast.py +++ b/parol6/server/status_broadcast.py @@ -2,6 +2,7 @@ import logging import socket +import secrets import sys import time @@ -61,6 +62,8 @@ def __init__( self._send_failures = 0 self._max_send_failures = 3 self._last_fail_log_time = 0.0 + self._session_id = secrets.randbits(64) or 1 + self._seq = 0 self._setup_socket() @@ -207,7 +210,10 @@ def tick(self) -> None: if cache.age_s() > self._stale_s: return - payload = cache.to_binary() + payload = cache.to_binary( + session_id=self._session_id, seq=self._seq, mono_time_ns=time.monotonic_ns() + ) + self._seq += 1 sock = self._sock if sock is None: self._switch_to_unicast() diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index 32f5758..a778884 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -192,10 +192,6 @@ def __init__(self) -> None: self._queued_segments: int = 0 self._queued_duration: float = 0.0 - # Binary cache - self._binary_cache: bytes = b"" - self._binary_dirty: bool = True - # Change-detection caches to avoid expensive recomputation when inputs unchanged self._last_pos_in: np.ndarray = np.zeros((6,), dtype=np.int32) self._last_io_buf: np.ndarray = np.zeros((5,), dtype=np.uint8) @@ -460,8 +456,6 @@ def update_from_state(self, state: ControllerState) -> None: if state.shapes_version != self._last_shapes_version: self._last_shapes_version = state.shapes_version self._sync_ik_geometry(SyncShapes(shapes=tuple(state.shapes))) - # World changed → broadcast the new epoch so displays re-query. - self._binary_dirty = True if pos_changed or tool_changed: self.pose[:] = get_fkine_flat_mm(state) @@ -517,7 +511,7 @@ def update_from_state(self, state: ControllerState) -> None: self._last_tool_positions = ts.positions # Poll for async IK results (non-blocking, zero-alloc) - ik_changed = self._poll_ik_results() + self._poll_ik_results() action_changed = ( self._action_current != state.action_current @@ -548,15 +542,12 @@ def update_from_state(self, state: ControllerState) -> None: # One scalar pass keeps the 100Hz path allocation-free; the per-joint # bits feed both the aggregate `homed` and the homing-progress view. homed = True - homing_changed = False joints_homed = self._joints_homed for i in range(6): bit = 1 if state.Homed_in[i] else 0 if not bit: homed = False - if joints_homed[i] != bit: - joints_homed[i] = bit - homing_changed = True + joints_homed[i] = bit homed_changed = self._homed != homed if homed_changed: self._homed = homed @@ -565,7 +556,6 @@ 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 @@ -573,14 +563,12 @@ def update_from_state(self, state: ControllerState) -> None: 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 - if self._homing_step != step: - self._homing_step = step - homing_changed = True + self._homing_step = ( + state.homing_step if state.action_current == "HomeCommand" else 0 + ) collision_changed = ( self._collision_active != state.collision_active @@ -609,66 +597,47 @@ def update_from_state(self, state: ControllerState) -> None: self._p99_period_s = state.p99_period_s self._overruns = state.overrun_count - # Mark binary cache dirty if anything changed - if ( - pos_changed - or tool_changed - or tool_status_changed - or io_changed - or spd_changed - or ik_changed - or action_changed - or queue_changed - or error_changed - or homed_changed - or enabled_changed - or homing_changed - or collision_changed - or depth_changed - or loop_changed - or faults_changed - ): - self._binary_dirty = True - - def to_binary(self) -> bytes: + def to_binary( + self, *, session_id: int = 0, seq: int = 0, mono_time_ns: int = 0 + ) -> bytes: """Return the msgpack-encoded STATUS payload.""" - if self._binary_dirty: - from parol6.server.transports.transport_factory import is_simulation_mode - - self._binary_cache = pack_status( - self.pose, - self.angles_deg, - self.speeds_rad_s, - self.io, - self._action_current, - self._action_state, - self._joint_en, - self._cart_en_wrf, - self._cart_en_trf, - self._executing_index, - self._completed_index, - self._last_checkpoint, - self._error, - self._queued_segments, - self._queued_duration, - self._action_params, - self.tool_status, - self.tcp_speed, - simulator_active=is_simulation_mode(), - collision_active=self._collision_active, - collision_pairs=self._collision_pairs, - scene_epoch=self._last_shapes_version, - accepted_index=self._accepted_index, - homed=self._homed, - enabled=self._enabled, - homing_step=self._homing_step, - 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 + from parol6.server.transports.transport_factory import is_simulation_mode + + return pack_status( + self.pose, + self.angles_deg, + self.speeds_rad_s, + self.io, + self._action_current, + self._action_state, + self._joint_en, + self._cart_en_wrf, + self._cart_en_trf, + self._executing_index, + self._completed_index, + self._last_checkpoint, + self._error, + self._queued_segments, + self._queued_duration, + self._action_params, + self.tool_status, + self.tcp_speed, + simulator_active=is_simulation_mode(), + collision_active=self._collision_active, + collision_pairs=self._collision_pairs, + scene_epoch=self._last_shapes_version, + accepted_index=self._accepted_index, + homed=self._homed, + enabled=self._enabled, + homing_step=self._homing_step, + joints_homed=self._joints_homed, + p99_period_s=self._p99_period_s, + overruns=self._overruns, + drive_faults=self._drive_faults, + session_id=session_id, + seq=seq, + mono_time_ns=mono_time_ns, + ) def mark_serial_observed(self) -> None: """Mark that a fresh serial frame was observed just now.""" diff --git a/tests/integration/test_status_rate.py b/tests/integration/test_status_rate.py index dac25f3..39abce5 100644 --- a/tests/integration/test_status_rate.py +++ b/tests/integration/test_status_rate.py @@ -23,7 +23,14 @@ 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(): + previous = None + async for status in client.stream_status(): + assert status.session_id > 0 and status.mono_time_ns > 0 + if previous is not None: + assert status.session_id == previous.session_id + assert status.seq > previous.seq + assert status.mono_time_ns > previous.mono_time_ns + previous = status if seen == 0: start = time.perf_counter() seen += 1 diff --git a/tests/unit/test_status_timing.py b/tests/unit/test_status_timing.py new file mode 100644 index 0000000..3fccba1 --- /dev/null +++ b/tests/unit/test_status_timing.py @@ -0,0 +1,41 @@ +"""Timing metadata survives snapshots and malformed packets cannot invent it.""" + +import msgspec + +from parol6.protocol.wire import StatusBuffer, decode_status_bin_into +from parol6.server.status_cache import StatusCache + + +def test_status_metadata_rejects_malformed_fields_and_clears_unavailable_metadata(): + cache = StatusCache() + try: + raw = cache.to_binary(session_id=2**64 - 1, seq=7, mono_time_ns=123456789) + buffer = StatusBuffer() + assert decode_status_bin_into(raw, buffer) + frozen = buffer.copy() + raw = cache.to_binary(session_id=9, seq=0, mono_time_ns=1) + assert decode_status_bin_into(raw, buffer) + assert (frozen.session_id, frozen.seq, frozen.mono_time_ns) == ( + 2**64 - 1, + 7, + 123456789, + ) + assert (buffer.session_id, buffer.seq, buffer.mono_time_ns) == (9, 0, 1) + packet = msgspec.msgpack.decode(raw) + # One slot later than the fields above them: the protocol version + # leads the message. + for field in (31, 32, 33): + for invalid in (True, -1, 1.5, float("nan"), "1", None): + changed = list(packet) + changed[field] = invalid + assert not decode_status_bin_into( + msgspec.msgpack.encode(changed), buffer + ) + for length in (32, 33): + assert not decode_status_bin_into( + msgspec.msgpack.encode(packet[:length]), buffer + ) + assert decode_status_bin_into(msgspec.msgpack.encode(packet[:31]), buffer) + assert (buffer.session_id, buffer.seq, buffer.mono_time_ns) == (0, 0, 0) + finally: + cache.close()