#!/usr/bin/env python3 """Session-gate local xTELE data and send it to the robot through OmniSocket. The raw/local-data stream remains authoritative for arm positions, physical buttons and hardware diagnostics. When configured, only the processed hand target from xTELE's command stream is merged into that raw frame. This keeps xTELE's built-in combo/gesture state machine and BrainCoRevo2 poses without letting unrelated UI, walking or base commands reach the dual-arm bridge. The physical left-Z + right-C hold is evaluated locally: idle frames never leave EAI, while each active interval carries a unique START/ACTIVE/STOP ID. """ from __future__ import annotations import argparse import copy import json import math import os from pathlib import Path import signal import struct import threading import time from typing import Any import uuid import zmq from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_ERROR, Session MAGIC = b"TG3A" HEADER = struct.Struct("!4sQQI") MAX_PAYLOAD_BYTES = 1_048_576 TELEOP_PROTOCOL_VERSION = 2 class XteleSender: def __init__(self, args: argparse.Namespace) -> None: if args.start_stop_hold_s <= 0.0: raise ValueError("start/stop hold time must be positive") if args.combo_release_s <= 0.0: raise ValueError("combo release confirmation time must be positive") if args.start_marker_frames <= 0: raise ValueError("start marker frame count must be positive") if args.source_timeout_s <= 0.0: raise ValueError("source timeout must be positive") if args.max_feedback_age_ms <= 0.0: raise ValueError("maximum KCP feedback age must be positive") if args.max_pending_frames <= 0: raise ValueError("maximum pending frame count must be positive") self.args = args self.stop = False self.session: Session | None = None self.session_connected_at = 0.0 self.started_at = time.time() self.last_status_at = 0.0 self.last_frame_at = 0.0 self.last_source_frame_at = 0.0 self.last_source_timestamp: float | None = None self.last_source_timestamp_advanced_at = 0.0 self.last_command_at = 0.0 self.last_error = "" self.teleop_active = False self.teleop_session_id: str | None = None self.teleop_session_seq = 0 self.packet_sequence = time.time_ns() self.start_markers_remaining = 0 self.combo_started_at: float | None = None self.combo_release_started_at: float | None = None # A service restart must never turn an already-held combo into a start. self.require_combo_release = True # START is a two-phase transaction. A physical 3-second hold creates # a pending request; only a successful Hub connection followed by a # newer raw frame that still shows Z+C pressed may send START. self.start_pending = False self.start_wait_fresh_after_connect = False self.start_fresh_barrier_timestamp: float | None = None self.latest_combo_state: bool | None = None self.last_start_failure = "" # Session.connect() may wait for the Hub registration timeout. Run it # outside the input loop so a release or stale xTELE source is still # observed immediately while an attempt is in flight. self.connect_attempt_generation = 0 self.connect_thread: threading.Thread | None = None self.connect_result_lock = threading.Lock() self.connect_result: tuple[int, Session | None, str] | None = None # Keep xTELE's processed right-hand stream isolated for the complete # right-B press and stable-release transaction. The robot runs the # authoritative three-second gesture toggle from the raw B state. self.right_b_merge_suppressed = False self.right_b_release_started_at: float | None = None self.right_b_recovery_started_at: float | None = None self.right_b_processed_baseline: object | None = None self.right_b_baseline_candidate: object | None = None self.right_b_baseline_candidate_started_at: float | None = None self.counters = { "connected": 0, "reconnects": 0, "frames_received": 0, "frames_sent": 0, "bytes_received": 0, "bytes_sent": 0, "dropped_malformed": 0, "command_frames_received": 0, "command_frames_accepted": 0, "command_frames_malformed": 0, "command_hand_merges": 0, "remote_errors": 0, "frames_suppressed_inactive": 0, "teleop_starts": 0, "teleop_stops": 0, "teleop_stop_send_failures": 0, "teleop_aborts": 0, "teleop_start_requests": 0, "start_connect_attempts": 0, "start_connect_failures": 0, "start_send_failures": 0, "start_pending_cancels": 0, "source_timestamp_errors": 0, "source_timestamp_resets": 0, } signal.signal(signal.SIGINT, self._request_stop) signal.signal(signal.SIGTERM, self._request_stop) def _request_stop(self, _signum: int, _frame: Any) -> None: self.stop = True def close_session(self) -> None: if self.session is not None: try: self.session.close() except OSError: pass self.session = None self.session_connected_at = 0.0 self.counters["connected"] = 0 def write_status(self, force: bool = False) -> None: now = time.monotonic() if not force and now - self.last_status_at < 0.5: return session_stats: dict[str, object] = {} kcp_stats: dict[str, object] = {} if self.session is not None: try: session_stats = self.session.stats() kcp_stats = self.session.kcp_stats() except OSError as exc: self.last_error = f"OmniSocket stats failed: {exc}" status = { "role": "xtele_sender", "server": self.args.server, "peer_id": self.args.peer_id, "target_peer": self.args.target_peer, "zmq_endpoint": self.args.zmq_endpoint, "cmd_zmq_endpoint": self.args.cmd_zmq_endpoint or None, "uptime_s": round(time.time() - self.started_at, 1), "last_frame_age_s": None if self.last_frame_at == 0.0 else round(time.monotonic() - self.last_frame_at, 4), "last_command_age_s": None if self.last_command_at == 0.0 else round(time.monotonic() - self.last_command_at, 4), "last_source_frame_age_s": None if self.last_source_frame_at == 0.0 else round(time.monotonic() - self.last_source_frame_at, 4), "last_source_timestamp": self.last_source_timestamp, "source_timestamp_advance_age_s": None if self.last_source_timestamp_advanced_at == 0.0 else round( time.monotonic() - self.last_source_timestamp_advanced_at, 4 ), "teleop_active": self.teleop_active, "teleop_session_id": self.teleop_session_id, "teleop_session_seq": self.teleop_session_seq, "teleop_combo_hold_s": 0.0 if self.combo_started_at is None else round(time.monotonic() - self.combo_started_at, 2), "teleop_require_combo_release": self.require_combo_release, "teleop_combo_release_hold_s": 0.0 if self.combo_release_started_at is None else round(time.monotonic() - self.combo_release_started_at, 2), "teleop_start_pending": self.start_pending, "teleop_start_waiting_fresh_frame": ( self.start_wait_fresh_after_connect ), "teleop_start_connect_inflight": self._connect_inflight(), "teleop_latest_combo_state": self.latest_combo_state, "last_start_failure": self.last_start_failure, "right_b_processed_merge_suppressed": self.right_b_merge_suppressed, "right_b_merge_release_hold_s": 0.0 if self.right_b_release_started_at is None else round(time.monotonic() - self.right_b_release_started_at, 2), "right_b_processed_recovery_hold_s": 0.0 if self.right_b_recovery_started_at is None else round(time.monotonic() - self.right_b_recovery_started_at, 2), "application_data_sending": ( self.teleop_active and self.session is not None ), "last_error": self.last_error, "session_stats": session_stats, "kcp_stats": kcp_stats, **self.counters, "updated_unix_s": time.time(), } path = Path(self.args.status_file) try: path.parent.mkdir(parents=True, exist_ok=True) temporary = path.with_suffix(path.suffix + ".tmp") temporary.write_text(json.dumps(status, indent=2) + "\n") os.replace(temporary, path) except OSError: pass self.last_status_at = now def _connect_inflight(self) -> bool: thread = self.connect_thread return thread is not None and thread.is_alive() def _connect_worker(self, generation: int) -> None: session: Session | None = None failure = "" try: session = Session() session.connect( server_addr=self.args.server, peer_id=self.args.peer_id, **CONTROL_DEFAULTS, ) except OSError as exc: failure = f"OmniSocket connect failed: {exc}" if session is not None: try: session.close() except OSError: pass session = None except Exception as exc: failure = f"unexpected OmniSocket connect failure: {exc}" if session is not None: try: session.close() except OSError: pass session = None with self.connect_result_lock: self.connect_result = (generation, session, failure) def _start_connect_attempt(self) -> bool: if self._connect_inflight(): return False with self.connect_result_lock: # A worker can publish its result just after the main loop polls # it. Let the next poll adopt/discard that result instead of # overwriting (and leaking) a connected Session. if self.connect_result is not None: return False self.connect_attempt_generation += 1 generation = self.connect_attempt_generation thread = threading.Thread( target=self._connect_worker, args=(generation,), name="tg3-omnisocket-start-connect", daemon=True, ) self.connect_thread = thread thread.start() return True def _invalidate_connect_attempt(self) -> None: # The SDK connect call is not cancellable. Changing the generation # makes its eventual result unusable; the main loop will close it. self.connect_attempt_generation += 1 def _poll_connect_result(self, now: float) -> str | None: with self.connect_result_lock: result = self.connect_result self.connect_result = None if result is None: return None generation, session, failure = result thread = self.connect_thread if thread is not None: thread.join(timeout=0.0) self.connect_thread = None if generation != self.connect_attempt_generation or not self.start_pending: if session is not None: try: session.close() except OSError: pass return "discarded" if session is None: self.counters["start_connect_failures"] += 1 self.last_error = failure or "OmniSocket Hub connection failed" # One physical hold makes exactly one connection attempt. A Hub # failure consumes no robot command, but it does require a stable # release before the operator may make a new three-second hold. self._cancel_pending_start(self.last_error) self.write_status(force=True) return "failed" self.session = session self.session_connected_at = now self.counters["connected"] = 1 self.counters["reconnects"] += 1 self.last_error = "" # Do not trust anything queued while registration was in progress. # Two advancing raw timestamps after adoption prove the source is # still live before START can leave EAI. self.start_wait_fresh_after_connect = True self.start_fresh_barrier_timestamp = None self.write_status(force=True) return "connected" def _drain_responses(self) -> None: if self.session is None: return try: while True: response = self.session.recv(timeout_ms=0) if response is None: break _from_peer, msg_type, payload = response if msg_type == MSG_TYPE_ERROR: self.counters["remote_errors"] += 1 detail = payload.decode("utf-8", errors="replace") self._abort_teleop(f"OmniSocket remote error: {detail}") self.close_session() return except OSError as exc: self._abort_teleop(f"OmniSocket receive failed: {exc}") self.close_session() @staticmethod def _valid_hand_side(value: object) -> bool: if isinstance(value, bool): return False if isinstance(value, (int, float)): numeric = float(value) return math.isfinite(numeric) and -0.05 <= numeric <= 1.05 if not isinstance(value, list) or len(value) != 6: return False try: numeric_values = [float(item) for item in value] except (TypeError, ValueError): return False return all( math.isfinite(item) and -0.05 <= item <= 1.05 for item in numeric_values ) @classmethod def _processed_hand_position( cls, command: dict[str, object] ) -> dict[str, object] | None: try: position = command["hand"]["position"] # type: ignore[index] left = position["left"] # type: ignore[index] right = position["right"] # type: ignore[index] except (KeyError, TypeError): return None if not cls._valid_hand_side(left) or not cls._valid_hand_side(right): return None return {"left": copy.deepcopy(left), "right": copy.deepcopy(right)} @staticmethod def _command_aligned_with_raw( command: dict[str, object], data: dict[str, object] ) -> bool: """Reject a processed 5001 target newer than the current raw frame.""" command_timestamp = command.get("timestamp") raw_timestamp = data.get("timestamp") if ( isinstance(command_timestamp, bool) or isinstance(raw_timestamp, bool) or not isinstance(command_timestamp, (int, float)) or not isinstance(raw_timestamp, (int, float)) ): return False command_value = float(command_timestamp) raw_value = float(raw_timestamp) return ( math.isfinite(command_value) and math.isfinite(raw_value) and command_value <= raw_value ) @staticmethod def _right_b_pressed(data: dict[str, object]) -> bool | None: """Return raw TS1P right-B, or ``None`` for malformed input.""" try: buttons = data["button"] right = buttons["right"] # type: ignore[index] value = right[1] # type: ignore[index] except (IndexError, KeyError, TypeError): return None if isinstance(value, bool): return value if isinstance(value, int) and value in (0, 1): return bool(value) return None @classmethod def _hand_targets_equivalent(cls, first: object, second: object) -> bool: """Compare normalized processed/raw hand targets with small jitter.""" if not cls._valid_hand_side(first) or not cls._valid_hand_side(second): return False if isinstance(first, (int, float)) and not isinstance(first, bool): if not isinstance(second, (int, float)) or isinstance(second, bool): return False return abs(float(first) - float(second)) <= 0.02 if not isinstance(first, list) or not isinstance(second, list): return False return all( abs(float(left) - float(right)) <= 0.02 for left, right in zip(first, second) ) @staticmethod def _raw_right_hand_target(data: dict[str, object]) -> object | None: try: hand = data["hand"] position = hand["position"] # type: ignore[index] return position["right"] # type: ignore[index] except (KeyError, TypeError): return None def _update_right_b_merge_gate( self, now: float, data: dict[str, object], processed_right: object | None, ) -> bool: """Suppress processed right hand until B release and target recovery.""" pressed = self._right_b_pressed(data) if pressed is True: self.right_b_merge_suppressed = True self.right_b_release_started_at = None self.right_b_recovery_started_at = None self.right_b_baseline_candidate = None self.right_b_baseline_candidate_started_at = None return True if pressed is None: # A malformed button sample may never clear an in-progress gate. self.right_b_merge_suppressed = True self.right_b_release_started_at = None self.right_b_recovery_started_at = None self.right_b_baseline_candidate = None self.right_b_baseline_candidate_started_at = None return True if not self.right_b_merge_suppressed: self.right_b_release_started_at = None self.right_b_recovery_started_at = None if not self._valid_hand_side(processed_right): self.right_b_baseline_candidate = None self.right_b_baseline_candidate_started_at = None return False if not self._hand_targets_equivalent( processed_right, self.right_b_baseline_candidate ): self.right_b_baseline_candidate = copy.deepcopy(processed_right) self.right_b_baseline_candidate_started_at = now return False if self.right_b_baseline_candidate_started_at is None: self.right_b_baseline_candidate_started_at = now return False baseline_seconds = max(0.1, float(self.args.cmd_max_age_s)) if now - self.right_b_baseline_candidate_started_at >= baseline_seconds: self.right_b_processed_baseline = copy.deepcopy(processed_right) return False if self.right_b_release_started_at is None: self.right_b_release_started_at = now self.right_b_recovery_started_at = None return True if now - self.right_b_release_started_at < self.args.combo_release_s: self.right_b_recovery_started_at = None return True # A stable raw release alone is insufficient: xTELE may retain a # processed B gesture after the release edge. Re-enable the processed # side only after fresh 5001 data continuously matches either its # pre-B baseline or the current raw scalar target. raw_right = self._raw_right_hand_target(data) recovered = self._hand_targets_equivalent( processed_right, self.right_b_processed_baseline ) or self._hand_targets_equivalent(processed_right, raw_right) if not recovered: self.right_b_recovery_started_at = None return True if self.right_b_recovery_started_at is None: self.right_b_recovery_started_at = now return True recovery_seconds = max(0.1, float(self.args.cmd_max_age_s)) if now - self.right_b_recovery_started_at < recovery_seconds: return True self.right_b_merge_suppressed = False self.right_b_release_started_at = None self.right_b_recovery_started_at = None self.right_b_processed_baseline = copy.deepcopy(processed_right) self.right_b_baseline_candidate = copy.deepcopy(processed_right) self.right_b_baseline_candidate_started_at = now return False @staticmethod def _select_processed_hand_position( raw_position: object, processed_position: dict[str, object] | None, *, suppress_right: bool, ) -> tuple[dict[str, object] | None, tuple[str, ...]]: """Select processed sides without mutating either source structure.""" if processed_position is None: return None, () if not suppress_right: return copy.deepcopy(processed_position), ("left", "right") # While right B is held, xTELE may emit its own right-hand gesture # before our separate three-second B latch fires on the robot. Keep # the authoritative raw 5003 right-hand value, but allow an unrelated # processed left-hand target to pass through. if not isinstance(raw_position, dict) or "right" not in raw_position: return None, () selected = copy.deepcopy(raw_position) selected["left"] = copy.deepcopy(processed_position["left"]) return selected, ("left",) @classmethod def _build_payload( cls, data: dict[str, object], command: dict[str, object] | None, session_id: str, session_seq: int, session_state: str, stop_reason: str = "", suppress_processed_right: bool | None = None, ) -> tuple[bytes, bool]: merged_sides: tuple[str, ...] = () try: # Build from a snapshot so callers retain the unmodified raw 5003 # frame even when a processed 5001 hand target is selected. payload_data = copy.deepcopy(data) position = cls._processed_hand_position(command) if command else None hand = payload_data.get("hand") right_b_pressed = cls._right_b_pressed(payload_data) if suppress_processed_right is None: suppress_processed_right = right_b_pressed is not False if isinstance(hand, dict): selected, merged_sides = cls._select_processed_hand_position( hand.get("position"), position, suppress_right=suppress_processed_right, ) if selected is not None: hand["position"] = selected metadata = payload_data.get("tg3_transport") if not isinstance(metadata, dict): metadata = {} payload_data["tg3_transport"] = metadata if merged_sides: metadata["processed_hand_from_xtele_cmd"] = True metadata["processed_hand_sides"] = list(merged_sides) assert command is not None metadata["xtele_cmd_timestamp"] = command.get("timestamp") else: metadata.pop("processed_hand_from_xtele_cmd", None) metadata.pop("processed_hand_sides", None) metadata.pop("xtele_cmd_timestamp", None) if suppress_processed_right and position is not None: metadata["processed_right_hand_suppressed_by_b"] = True else: metadata.pop("processed_right_hand_suppressed_by_b", None) # Always overwrite untrusted source metadata. The robot accepts # start/active/stop only from this sender and expected Omni peer. metadata["protocol_version"] = TELEOP_PROTOCOL_VERSION metadata["session_id"] = session_id metadata["session_seq"] = session_seq metadata["session_state"] = session_state if stop_reason: metadata["stop_reason"] = stop_reason else: metadata.pop("stop_reason", None) encoded = json.dumps( payload_data, ensure_ascii=False, separators=(",", ":") ).encode("utf-8") except (TypeError, ValueError): raise ValueError("cannot encode xTELE session payload") return encoded, bool(merged_sides) @staticmethod def _start_stop_pressed(data: dict[str, object]) -> bool | None: """Return explicit Z+C state, or ``None`` for malformed buttons.""" try: buttons = data["button"] left = buttons["left"] # type: ignore[index] right = buttons["right"] # type: ignore[index] if len(left) < 3 or len(right) < 3: # type: ignore[arg-type] return None left_z = left[2] # type: ignore[index] right_c = right[2] # type: ignore[index] except (IndexError, KeyError, TypeError): return None values: list[bool] = [] for value in (left_z, right_c): if isinstance(value, bool): values.append(value) elif isinstance(value, int) and value in (0, 1): values.append(bool(value)) else: return None return all(values) def _update_teleop_gate( self, now: float, data: dict[str, object] ) -> str | None: pressed = self._start_stop_pressed(data) self.latest_combo_state = pressed if pressed is None: self.combo_started_at = None self.combo_release_started_at = None return None if self.require_combo_release: self.combo_started_at = None if pressed: self.combo_release_started_at = None return None if self.combo_release_started_at is None: self.combo_release_started_at = now return None if ( now - self.combo_release_started_at >= self.args.combo_release_s ): self.require_combo_release = False self.combo_release_started_at = None return None self.combo_release_started_at = None if not pressed: self.combo_started_at = None return None if self.combo_started_at is None: self.combo_started_at = now if now - self.combo_started_at < self.args.start_stop_hold_s: return None self.combo_started_at = None self.combo_release_started_at = None self.require_combo_release = True if self.teleop_active: self.teleop_active = False self.counters["teleop_stops"] += 1 return "stop" self.start_pending = True self.teleop_session_id = uuid.uuid4().hex self.teleop_session_seq = 0 self.start_markers_remaining = self.args.start_marker_frames self.start_wait_fresh_after_connect = False self.last_start_failure = "" self.last_error = "" self.counters["teleop_start_requests"] += 1 return "start_pending" def _clear_pending_start_state(self) -> None: self.start_pending = False self.start_wait_fresh_after_connect = False self.start_fresh_barrier_timestamp = None def _cancel_pending_start(self, reason: str) -> None: if not self.start_pending: return self._invalidate_connect_attempt() self.close_session() self._clear_pending_start_state() self.teleop_active = False self.teleop_session_id = None self.teleop_session_seq = 0 self.start_markers_remaining = 0 self.combo_started_at = None self.require_combo_release = True self.last_start_failure = reason self.counters["start_pending_cancels"] += 1 def _pending_connect_ready(self, _now: float) -> bool: if not self.start_pending or self.latest_combo_state is not True: return False if self.session is not None: return False if self._connect_inflight(): return False return True def _commit_pending_start_sent(self, _now: float) -> None: self.counters["teleop_starts"] += 1 self.start_pending = False self.start_wait_fresh_after_connect = False self.start_fresh_barrier_timestamp = None self.teleop_active = True self.last_start_failure = "" def _abort_teleop(self, reason: str) -> None: if self.teleop_active or self.start_pending: self.counters["teleop_aborts"] += 1 self._invalidate_connect_attempt() self.teleop_active = False self._clear_pending_start_state() self.teleop_session_id = None self.teleop_session_seq = 0 self.start_markers_remaining = 0 self.combo_started_at = None self.require_combo_release = True self.last_error = reason self.last_start_failure = reason @staticmethod def _raw_source_timestamp(data: dict[str, object]) -> float | None: value = data.get("timestamp") if isinstance(value, bool) or not isinstance(value, (int, float)): return None timestamp = float(value) return timestamp if math.isfinite(timestamp) else None def _check_source_clock( self, now: float, data: dict[str, object] ) -> tuple[bool, str]: """Reject a missing, regressing or persistently frozen xTELE clock.""" timestamp = self._raw_source_timestamp(data) if timestamp is None: return False, "local xTELE timestamp is missing or malformed" previous = self.last_source_timestamp if previous is None or timestamp > previous: self.last_source_timestamp = timestamp self.last_source_timestamp_advanced_at = now return True, "" if timestamp == previous: if self.last_source_timestamp_advanced_at == 0.0: self.last_source_timestamp_advanced_at = now return True, "" if ( now - self.last_source_timestamp_advanced_at <= self.args.source_timeout_s ): return True, "" return False, "local xTELE timestamp stopped advancing" if not self.teleop_active and not self.start_pending: # A producer restart while idle is harmless only after a fresh # release. Reset the clock baseline but never inherit a held # Z+C combination across that restart. self.last_source_timestamp = timestamp self.last_source_timestamp_advanced_at = now self.combo_started_at = None self.combo_release_started_at = None self.require_combo_release = True self.latest_combo_state = None self.counters["source_timestamp_resets"] += 1 return True, "" return False, "local xTELE timestamp moved backwards" def _check_pending_start_freshness( self, data: dict[str, object] ) -> tuple[str, str]: """Gate START on two advancing frames received after connect adoption.""" if not self.start_wait_fresh_after_connect: return "ready", "" source_timestamp = self._raw_source_timestamp(data) if source_timestamp is None: return ( "cancel", "pending START cannot prove a fresh xTELE timestamp", ) if self.start_fresh_barrier_timestamp is None: self.start_fresh_barrier_timestamp = source_timestamp return "wait", "" if source_timestamp < self.start_fresh_barrier_timestamp: return ( "cancel", "pending START xTELE timestamp moved backwards", ) if source_timestamp == self.start_fresh_barrier_timestamp: # xTELE runs near 100 Hz while its millisecond timestamp can repeat # for adjacent frames. Keep waiting; never treat a duplicate as # proof of freshness and never force a new physical button cycle. return "wait", "" self.start_wait_fresh_after_connect = False self.start_fresh_barrier_timestamp = None return "ready", "" def _try_send_payload(self, payload: bytes) -> tuple[bool, str]: if self.session is None: return False, "OmniSocket session is unavailable" unhealthy = self._session_unhealthy_reason() if unhealthy: return False, unhealthy self.packet_sequence += 1 packet = HEADER.pack( MAGIC, self.packet_sequence, time.time_ns(), len(payload) ) + payload try: self.session.send(to=self.args.target_peer, data=packet) except OSError as exc: return False, f"OmniSocket send failed: {exc}" self.counters["frames_sent"] += 1 self.counters["bytes_sent"] += len(payload) self.last_frame_at = time.monotonic() self.last_error = "" return True, "" def _send_payload(self, payload: bytes) -> bool: sent, reason = self._try_send_payload(payload) if sent: return True self._abort_teleop(reason) self.close_session() return False def _session_unhealthy_reason(self) -> str | None: if self.session is None: return "OmniSocket session is unavailable" try: session_stats = self.session.stats() kcp_stats = self.session.kcp_stats() if int(session_stats.get("connected", 0)) != 1 or int( session_stats.get("registered", 0) ) != 1: return "OmniSocket session lost registration" pending = int(kcp_stats.get("snd_queue", 0)) + int( kcp_stats.get("snd_buffer", 0) ) if pending > self.args.max_pending_frames: return ( f"OmniSocket pending queue reached {pending} frames; " "session invalidated to prevent stale replay" ) feedback_age_ms = float(kcp_stats.get("last_feedback_age_ms", 0.0)) connection_age_s = time.monotonic() - self.session_connected_at if ( connection_age_s >= 0.5 and feedback_age_ms > self.args.max_feedback_age_ms ): return ( f"OmniSocket feedback stale for {feedback_age_ms:.0f} ms; " "session invalidated to prevent stale replay" ) except (OSError, TypeError, ValueError) as exc: return f"cannot verify OmniSocket session health: {exc}" return None def _flush_session(self, timeout_s: float = 0.75) -> None: """Bound the final STOP flush, without sending another business frame.""" deadline = time.monotonic() + timeout_s while self.session is not None and time.monotonic() < deadline: try: stats = self.session.kcp_stats() pending = int(stats.get("snd_queue", 0)) + int( stats.get("snd_buffer", 0) ) except (OSError, TypeError, ValueError): return if pending == 0: return self._drain_responses() time.sleep(0.01) def _finish_stop_session(self) -> None: """Close an operator STOP locally even if its final frame failed.""" self.teleop_active = False self.teleop_session_id = None self.teleop_session_seq = 0 self.start_markers_remaining = 0 self._flush_session() self.close_session() def run(self) -> int: context = zmq.Context() source = context.socket(zmq.SUB) source.setsockopt(zmq.SUBSCRIBE, b"") source.setsockopt(zmq.CONFLATE, 1) source.setsockopt(zmq.RCVHWM, 1) source.setsockopt(zmq.LINGER, 0) source.connect(self.args.zmq_endpoint) poller = zmq.Poller() poller.register(source, zmq.POLLIN) command_source = None if self.args.cmd_zmq_endpoint: command_source = context.socket(zmq.SUB) command_source.setsockopt(zmq.SUBSCRIBE, b"") command_source.setsockopt(zmq.CONFLATE, 1) command_source.setsockopt(zmq.RCVHWM, 1) command_source.setsockopt(zmq.LINGER, 0) command_source.connect(self.args.cmd_zmq_endpoint) poller.register(command_source, zmq.POLLIN) latest_command: dict[str, object] | None = None try: while not self.stop: events = dict(poller.poll(100)) # Connection attempts run in a worker so this loop can keep # observing physical releases and source freshness. Only the # main thread ever adopts a completed Session. self._poll_connect_result(time.monotonic()) if command_source is not None and command_source in events: command_raw = command_source.recv() self.counters["command_frames_received"] += 1 try: parsed_command = json.loads(command_raw) if not isinstance(parsed_command, dict): raise ValueError("xTELE command must be a JSON object") if self._processed_hand_position(parsed_command) is None: raise ValueError( "xTELE command has no valid bilateral hand target" ) latest_command = parsed_command self.last_command_at = time.monotonic() self.counters["command_frames_accepted"] += 1 except (TypeError, ValueError, json.JSONDecodeError): self.counters["command_frames_malformed"] += 1 if source not in events: now = time.monotonic() if ( self.last_source_frame_at != 0.0 and now - self.last_source_frame_at > self.args.source_timeout_s ): self.combo_started_at = None self.combo_release_started_at = None if self.start_pending: self._cancel_pending_start( "local xTELE source became stale during START" ) if self.teleop_active: self._abort_teleop( "local xTELE source became stale; a new Z+C hold " "is required" ) self.close_session() self._drain_responses() self.write_status() continue raw = source.recv() self.counters["frames_received"] += 1 self.counters["bytes_received"] += len(raw) if not raw or len(raw) > MAX_PAYLOAD_BYTES: self.combo_started_at = None self.combo_release_started_at = None if self.start_pending: self._cancel_pending_start( "malformed local xTELE frame during START" ) if self.teleop_active: self._abort_teleop( "malformed local xTELE frame during active session" ) self.close_session() self.counters["dropped_malformed"] += 1 continue try: data = json.loads(raw) if not isinstance(data, dict): raise ValueError("xTELE raw frame must be a JSON object") except (TypeError, ValueError, json.JSONDecodeError): # A malformed/frozen frame may never contribute time to a # physical three-second start/stop hold. self.combo_started_at = None self.combo_release_started_at = None if self.start_pending: self._cancel_pending_start( "malformed local xTELE JSON during START" ) if self.teleop_active: self._abort_teleop( "malformed local xTELE JSON during active session" ) self.close_session() self.counters["dropped_malformed"] += 1 continue now = time.monotonic() self.last_source_frame_at = now clock_ok, clock_reason = self._check_source_clock(now, data) if not clock_ok: self.combo_started_at = None self.combo_release_started_at = None self.latest_combo_state = None self.counters["source_timestamp_errors"] += 1 self.counters["dropped_malformed"] += 1 if self.start_pending: self._cancel_pending_start(clock_reason) if self.teleop_active: self._abort_teleop(clock_reason) self.close_session() continue transition = self._update_teleop_gate(now, data) command = None if ( latest_command is not None and now - self.last_command_at <= self.args.cmd_max_age_s and self._command_aligned_with_raw(latest_command, data) ): command = latest_command processed_position = ( self._processed_hand_position(command) if command is not None else None ) processed_right = ( None if processed_position is None else processed_position["right"] ) suppress_processed_right = self._update_right_b_merge_gate( now, data, processed_right ) if self.start_pending and self.latest_combo_state is not True: if self.latest_combo_state is False: pending_cancel_reason = ( "Z+C released before START reached the robot" ) else: pending_cancel_reason = ( "malformed Z+C state during pending START" ) self._cancel_pending_start(pending_cancel_reason) if self.start_pending: if self.session is None: if not self._pending_connect_ready(now): self.counters["frames_suppressed_inactive"] += 1 self._drain_responses() self.write_status() continue if self._start_connect_attempt(): self.counters["start_connect_attempts"] += 1 self.counters["frames_suppressed_inactive"] += 1 self.write_status(force=True) continue freshness, freshness_reason = ( self._check_pending_start_freshness(data) ) if freshness == "wait": self.counters["frames_suppressed_inactive"] += 1 self.write_status() continue if freshness == "cancel": self._cancel_pending_start(freshness_reason) self.counters["frames_suppressed_inactive"] += 1 continue session_id = self.teleop_session_id if not session_id: self._cancel_pending_start( "pending START session ID is unavailable" ) self.counters["frames_suppressed_inactive"] += 1 continue self.teleop_session_seq += 1 try: payload, merged = self._build_payload( data, command, session_id, self.teleop_session_seq, "start", "", suppress_processed_right, ) except ValueError: self._cancel_pending_start( "cannot encode a fresh pending START frame" ) self.counters["dropped_malformed"] += 1 continue if len(payload) > MAX_PAYLOAD_BYTES: self._cancel_pending_start( "pending START frame exceeds maximum size" ) self.counters["dropped_malformed"] += 1 continue sent, failure = self._try_send_payload(payload) if not sent: self.counters["start_send_failures"] += 1 self._cancel_pending_start(failure) self.counters["frames_suppressed_inactive"] += 1 continue if merged: self.counters["command_hand_merges"] += 1 self.start_markers_remaining -= 1 self._commit_pending_start_sent(now) self._drain_responses() self.write_status(force=True) continue if not self.teleop_active and transition != "stop": self.counters["frames_suppressed_inactive"] += 1 self._drain_responses() self.write_status() continue session_id = self.teleop_session_id if not session_id: self._abort_teleop("teleoperation session ID is unavailable") self.counters["frames_suppressed_inactive"] += 1 continue if self.session is None: self._abort_teleop( "active OmniSocket session is unavailable; a new Z+C " "cycle is required" ) self.counters["frames_suppressed_inactive"] += 1 continue self.teleop_session_seq += 1 if transition == "stop": session_state = "stop" stop_reason = "operator" elif self.start_markers_remaining > 0: session_state = "start" stop_reason = "" else: session_state = "active" stop_reason = "" stop_requested = transition == "stop" try: try: payload, merged = self._build_payload( data, command, session_id, self.teleop_session_seq, session_state, stop_reason, suppress_processed_right, ) except ValueError: self.counters["dropped_malformed"] += 1 if stop_requested: self.counters["teleop_stop_send_failures"] += 1 self.last_error = "cannot encode operator STOP frame" continue if merged: self.counters["command_hand_merges"] += 1 if len(payload) > MAX_PAYLOAD_BYTES: self.counters["dropped_malformed"] += 1 if stop_requested: self.counters["teleop_stop_send_failures"] += 1 self.last_error = ( "operator STOP frame exceeds maximum size" ) continue sent = self._send_payload(payload) if not sent and stop_requested: self.counters["teleop_stop_send_failures"] += 1 if sent and session_state == "start": self.start_markers_remaining -= 1 finally: if stop_requested: # STOP is final even when encode/send fails. The robot # then falls back to its input timeout; EAI must never # retain a registered, locally inactive Session. self._finish_stop_session() self._drain_responses() self.write_status() finally: self._invalidate_connect_attempt() source.close() if command_source is not None: command_source.close() context.term() thread = self.connect_thread if thread is not None: thread.join(timeout=3.5) self._poll_connect_result(time.monotonic()) self.close_session() self.write_status(force=True) return 0 def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--server", required=True) parser.add_argument("--peer-id", required=True) parser.add_argument("--target-peer", required=True) parser.add_argument("--zmq-endpoint", required=True) parser.add_argument( "--cmd-zmq-endpoint", default="", help=( "optional xTELE processed-command PUB endpoint; only its bilateral " "hand.position target is merged into the raw frame" ), ) parser.add_argument("--cmd-max-age-s", type=float, default=0.25) parser.add_argument("--source-timeout-s", type=float, default=0.25) parser.add_argument("--max-feedback-age-ms", type=float, default=500.0) parser.add_argument("--max-pending-frames", type=int, default=100) parser.add_argument("--start-stop-hold-s", type=float, default=3.0) parser.add_argument( "--combo-release-s", type=float, default=0.5, help=( "require both combo buttons to remain released for this long " "before another start/stop hold can begin" ), ) parser.add_argument( "--start-marker-frames", type=int, default=50, help="repeat START for this many source frames before ACTIVE", ) parser.add_argument("--status-file", required=True) return parser.parse_args() if __name__ == "__main__": raise SystemExit(XteleSender(parse_args()).run())