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/async_client.py b/parol6/client/async_client.py index b5511df..0c66619 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -277,6 +277,7 @@ def skill_capabilities(self) -> frozenset[str]: return super().skill_capabilities | { "backend.parol6", "execution.speed", + "observation.timed", "tool.gripper", "io.digital", } diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 577144b..5cbb659 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -223,7 +223,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..0275caf 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -630,9 +630,11 @@ def update_from_state(self, state: ControllerState) -> None: ): 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: + if self._binary_dirty or session_id: from parol6.server.transports.transport_factory import is_simulation_mode self._binary_cache = pack_status( @@ -666,6 +668,9 @@ def to_binary(self) -> bytes: 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, ) self._binary_dirty = False return self._binary_cache 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()