meiqi/TG3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: make offline teleop start one-shot and rearmable

Browse Source
This commit is contained in:
LengedZhao
2026-08-08 16:30:50 +08:00
parent 1a79fca36f
commit 29030c0b25
5 changed files with 1154 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

558
tg3_omnisocket_transport/omnisocket_xtele_sender.py
View File

@@ -20,6 +20,7 @@ import os
from pathlib import Path
import signal
import struct
import threading
import time
from typing import Any
import uuid
@@ -56,6 +57,8 @@ class XteleSender:
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
@@ -67,6 +70,21 @@ class XteleSender:
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.
@@ -92,7 +110,15 @@ class XteleSender:
"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)
@@ -100,28 +126,6 @@ class XteleSender:
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:
@@ -161,6 +165,12 @@ class XteleSender:
"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,
@@ -171,6 +181,13 @@ class XteleSender:
"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
@@ -197,6 +214,106 @@ class XteleSender:
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
@@ -478,24 +595,38 @@ class XteleSender:
return encoded, bool(merged_sides)
@staticmethod
def _start_stop_pressed(data: dict[str, object]) -> bool:
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]
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
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:
@@ -528,33 +659,149 @@ class XteleSender:
self.counters["teleop_stops"] += 1
return "stop"
self.teleop_active = True
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.counters["teleop_starts"] += 1
return "start"
self.start_wait_fresh_after_connect = False
self.last_start_failure = ""
self.last_error = ""
self.counters["teleop_start_requests"] += 1
return "start_pending"
def _abort_teleop(self, reason: str) -> None:
if self.teleop_active:
self.counters["teleop_aborts"] += 1
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_error = reason
self.last_start_failure = reason
self.counters["start_pending_cancels"] += 1
def _send_payload(self, payload: bytes) -> bool:
if self.session is None:
self._abort_teleop("OmniSocket session is unavailable")
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:
self._abort_teleop(unhealthy)
self.close_session()
return False
return False, unhealthy
self.packet_sequence += 1
packet = HEADER.pack(
MAGIC, self.packet_sequence, time.time_ns(), len(payload)
@@ -562,14 +809,20 @@ class XteleSender:
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
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
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:
@@ -620,6 +873,16 @@ class XteleSender:
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)
@@ -643,6 +906,10 @@ class XteleSender:
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
@@ -669,6 +936,10 @@ class XteleSender:
):
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 "
@@ -683,6 +954,17 @@ class XteleSender:
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
@@ -695,11 +977,33 @@ class XteleSender:
# 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
@@ -723,6 +1027,86 @@ class XteleSender:
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()
@@ -735,10 +1119,10 @@ class XteleSender:
self.counters["frames_suppressed_inactive"] += 1
continue
if self.session is None and not self.connect():
if self.session is None:
self._abort_teleop(
"cannot start teleoperation because OmniSocket Hub is "
"unavailable; release Z+C before retrying"
"active OmniSocket session is unavailable; a new Z+C "
"cycle is required"
)
self.counters["frames_suppressed_inactive"] += 1
continue
@@ -753,44 +1137,58 @@ class XteleSender:
else:
session_state = "active"
stop_reason = ""
stop_requested = transition == "stop"
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
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 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()
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