839 lines
34 KiB
Python
Executable File
839 lines
34 KiB
Python
Executable File
#!/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())
|