#!/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 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_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 # 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_aborts": 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 connect(self) -> bool: self.close_session() session = Session() try: session.connect( server_addr=self.args.server, peer_id=self.args.peer_id, **CONTROL_DEFAULTS, ) except OSError as exc: self.last_error = f"OmniSocket connect failed: {exc}" session.close() self.write_status(force=True) return False self.session = session self.session_connected_at = time.monotonic() self.counters["connected"] = 1 self.counters["reconnects"] += 1 self.last_error = "" self.write_status(force=True) return 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), "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), "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 _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: try: buttons = data["button"] left = buttons["left"] # type: ignore[index] right = buttons["right"] # type: ignore[index] return ( len(left) >= 3 # type: ignore[arg-type] and len(right) >= 3 # type: ignore[arg-type] and bool(left[2]) # type: ignore[index] and bool(right[2]) # type: ignore[index] ) except (KeyError, TypeError): return False def _update_teleop_gate( self, now: float, data: dict[str, object] ) -> str | None: pressed = self._start_stop_pressed(data) 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.teleop_active = True self.teleop_session_id = uuid.uuid4().hex self.teleop_session_seq = 0 self.start_markers_remaining = self.args.start_marker_frames self.counters["teleop_starts"] += 1 return "start" def _abort_teleop(self, reason: str) -> None: if self.teleop_active: self.counters["teleop_aborts"] += 1 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_error = reason def _send_payload(self, payload: bytes) -> bool: if self.session is None: self._abort_teleop("OmniSocket session is unavailable") return False unhealthy = self._session_unhealthy_reason() if unhealthy: self._abort_teleop(unhealthy) self.close_session() return False 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: self._abort_teleop(f"OmniSocket send failed: {exc}") self.close_session() return False self.counters["frames_sent"] += 1 self.counters["bytes_sent"] += len(payload) self.last_frame_at = time.monotonic() self.last_error = "" return True 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 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)) 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.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.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 self.counters["dropped_malformed"] += 1 continue now = time.monotonic() self.last_source_frame_at = now 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 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 and not self.connect(): self._abort_teleop( "cannot start teleoperation because OmniSocket Hub is " "unavailable; release Z+C before retrying" ) 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 = "" 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 continue if merged: self.counters["command_hand_merges"] += 1 if len(payload) > MAX_PAYLOAD_BYTES: self.counters["dropped_malformed"] += 1 continue sent = self._send_payload(payload) if sent and session_state == "start": self.start_markers_remaining -= 1 if transition == "stop": # STOP is the final xTELE business frame. The underlying # Session is flushed for bounded delivery and then closed; # the next physical START creates a fresh registration. self.teleop_session_id = None self.teleop_session_seq = 0 self.start_markers_remaining = 0 self._flush_session() self.close_session() self._drain_responses() self.write_status() finally: source.close() if command_source is not None: command_source.close() context.term() 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())