#!/usr/bin/env python3 """Local TS1P isomorphic-arm bridge for TianGong 3.0. Arm targets use the vendor ``/encoder_identical_joint`` input, while guarded HBWALK velocity commands use the vendor ``/hric/robot/cmd_vel`` input. This bridge never changes the robot FSM; HBWALK must already be active and running. """ from __future__ import annotations import argparse import json import math import os import struct import threading import time import tomllib import uuid from dataclasses import dataclass from pathlib import Path from typing import Any import rclpy import zmq from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti from diagnostic_msgs.msg import DiagnosticStatus from geometry_msgs.msg import TwistStamped from rclpy.node import Node from ros2_bridge_msgs.msg import ArmStatus, HeadCtrl, MotorCtrl, RobotState from sensor_msgs.msg import JointState from std_msgs.msg import String from std_srvs.srv import Trigger from data_collection import RecordingToggleGate, left_joystick_pressed from gesture_toggle import ( GuardedMomentaryGesture, GestureToggle, normalized_pose_to_positions, right_a_pressed, right_b_pressed, select_hand_target, ) from head_control import HeadPitchController, head_pitch_command_axis, left_z_pressed JOINT_NAMES = [ *(f"left_joints_{i}" for i in range(7)), *(f"right_joints_{i}" for i in range(7)), ] MOTOR_IDS = [*range(11, 18), *range(21, 28)] HAND_SIDES = ("left", "right") OMNI_MAGIC = b"TG3A" OMNI_HEADER = struct.Struct("!4sQQI") TELEOP_PROTOCOL_VERSION = 2 @dataclass(frozen=True) class ArmSnapshot: data: dict[str, Any] received_at: float class LatestArmData: """Receive the latest industrial-PC sample without buffering old motion.""" def __init__(self, config: dict[str, Any]) -> None: self.cfg = config self.transport = str(config.get("transport", "zmq")).lower() if self.transport not in ("zmq", "omnisocket"): raise ValueError(f"unsupported iarm transport: {self.transport}") self.endpoint = str(config.get("iarm_endpoint", "")) if self.transport == "omnisocket": self.description = ( f"omnisocket://{config['omnisocket_server']}/" f"{config['omnisocket_peer_id']}" ) else: self.description = self.endpoint self._lock = threading.Lock() self._latest: ArmSnapshot | None = None self._last_error = f"waiting for the first {self.transport} sample" self._metrics: dict[str, Any] = { "transport": self.transport, "connected": False, "registered": False, "session_connects": 0, "idle_session_refreshes": 0, "idle_session_refresh_failures": 0, "frames_received": 0, "frames_accepted": 0, "dropped_sender": 0, "dropped_malformed": 0, "dropped_stale": 0, "dropped_sequence": 0, "raw_packet_age_ms": None, "packet_age_baseline_ms": None, "effective_packet_age_ms": None, "max_effective_packet_age_ms": None, } self._stop = threading.Event() self._thread = threading.Thread( target=self._run, name=f"iarm-{self.transport}", daemon=True ) def start(self) -> None: self._thread.start() def close(self) -> None: self._stop.set() self._thread.join(timeout=2.0) def get(self) -> tuple[ArmSnapshot | None, str]: with self._lock: return self._latest, self._last_error def metrics(self) -> dict[str, Any]: with self._lock: return dict(self._metrics) def _run(self) -> None: if self.transport == "omnisocket": self._run_omnisocket() else: self._run_zmq() def _run_zmq(self) -> None: context = zmq.Context.instance() sock = context.socket(zmq.SUB) sock.setsockopt(zmq.SUBSCRIBE, b"") sock.setsockopt(zmq.CONFLATE, 1) sock.setsockopt(zmq.RCVHWM, 1) sock.setsockopt(zmq.LINGER, 0) sock.connect(self.endpoint) poller = zmq.Poller() poller.register(sock, zmq.POLLIN) try: while not self._stop.is_set(): if sock not in dict(poller.poll(100)): continue try: raw = sock.recv(zmq.NOBLOCK) data = json.loads(raw) self._validate_shape(data) with self._lock: self._latest = ArmSnapshot( data=data, received_at=time.monotonic() ) self._last_error = "" self._metrics["connected"] = True self._metrics["frames_received"] += 1 self._metrics["frames_accepted"] += 1 except Exception as exc: # Keep receiving after one malformed packet. with self._lock: self._last_error = f"invalid ZMQ sample: {exc}" self._metrics["dropped_malformed"] += 1 finally: sock.close() def _run_omnisocket(self) -> None: try: from omnisocket import CONTROL_DEFAULTS, MSG_TYPE_BINARY, Session except ImportError as exc: with self._lock: self._last_error = f"cannot import OmniSocket extension: {exc}" return expected_sender = str(self.cfg["omnisocket_expected_sender"]) max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"]) idle_refresh_s = float( self.cfg.get("omnisocket_idle_session_refresh_s", 2.0) ) if idle_refresh_s < 0.0: raise ValueError("OmniSocket idle session refresh must be non-negative") last_sequence = 0 while not self._stop.is_set(): session = Session() baseline_ms: float | None = None try: session.connect( server_addr=str(self.cfg["omnisocket_server"]), peer_id=str(self.cfg["omnisocket_peer_id"]), **CONTROL_DEFAULTS, ) last_accepted_at = time.monotonic() session_stats = session.stats() with self._lock: self._metrics["connected"] = True self._metrics["registered"] = bool( int(session_stats.get("registered", 0)) == 1 ) self._metrics["session_connects"] += 1 self._last_error = "" while not self._stop.is_set(): message = session.recv(timeout_ms=100) if message is None: now = time.monotonic() if self._idle_session_refresh_due( now, last_accepted_at, idle_refresh_s ): # The deployed OmniSocket receiver has no idle Hub # heartbeat. After a Hub restart it may therefore # keep reporting connected although its server-side # registration is gone. Refresh with make-before- # break: register a replacement with the same peer # ID first, then close the old instance. The Hub # tracks registration instances, so closing the old # session does not unregister the replacement and # there is no healthy-idle routing gap. replacement = Session() try: replacement.connect( server_addr=str( self.cfg["omnisocket_server"] ), peer_id=str( self.cfg["omnisocket_peer_id"] ), **CONTROL_DEFAULTS, ) replacement_stats = replacement.stats() except Exception as exc: try: replacement.close() except OSError: pass last_accepted_at = now with self._lock: self._metrics[ "idle_session_refresh_failures" ] += 1 self._last_error = ( "OmniSocket idle registration refresh " f"failed: {exc}" ) continue old_session = session session = replacement baseline_ms = None last_accepted_at = time.monotonic() with self._lock: self._metrics["connected"] = True self._metrics["registered"] = bool( int( replacement_stats.get( "registered", 0 ) ) == 1 ) self._metrics["session_connects"] += 1 self._metrics[ "idle_session_refreshes" ] += 1 self._last_error = "" try: old_session.close() except OSError: pass continue messages = [message] while True: pending = session.recv(timeout_ms=0) if pending is None: break messages.append(pending) newest: tuple[int, bytes] | None = None now_ns = time.time_ns() for from_peer, msg_type, packet in messages: with self._lock: self._metrics["frames_received"] += 1 if from_peer != expected_sender: with self._lock: self._metrics["dropped_sender"] += 1 continue decoded = self._decode_omni_packet( msg_type, MSG_TYPE_BINARY, packet, now_ns, baseline_ms, ) if decoded is None: continue sequence, payload, raw_age_ms, effective_age_ms = decoded if baseline_ms is None or raw_age_ms < baseline_ms: baseline_ms = raw_age_ms effective_age_ms = 0.0 self._record_packet_age( raw_age_ms, baseline_ms, effective_age_ms ) if effective_age_ms > max_age_ms: with self._lock: self._metrics["dropped_stale"] += 1 continue if sequence <= last_sequence: with self._lock: self._metrics["dropped_sequence"] += 1 continue if newest is None or sequence > newest[0]: newest = (sequence, payload) if newest is None: continue sequence, payload = newest try: data = json.loads(payload) self._validate_shape(data) except Exception as exc: with self._lock: self._last_error = f"invalid OmniSocket sample: {exc}" self._metrics["dropped_malformed"] += 1 continue last_sequence = sequence with self._lock: self._latest = ArmSnapshot( data=data, received_at=time.monotonic() ) self._last_error = "" self._metrics["frames_accepted"] += 1 last_accepted_at = time.monotonic() except Exception as exc: with self._lock: self._metrics["connected"] = False self._metrics["registered"] = False self._last_error = f"OmniSocket connection failed: {exc}" finally: try: session.close() except OSError: pass with self._lock: self._metrics["connected"] = False self._metrics["registered"] = False self._stop.wait(1.0) @staticmethod def _idle_session_refresh_due( now: float, last_accepted_at: float, refresh_after_s: float ) -> bool: return ( refresh_after_s > 0.0 and now - last_accepted_at >= refresh_after_s ) def _decode_omni_packet( self, msg_type: int, binary_type: int, packet: bytes, now_ns: int, baseline_ms: float | None, ) -> tuple[int, bytes, float, float] | None: if msg_type != binary_type or len(packet) < OMNI_HEADER.size: with self._lock: self._metrics["dropped_malformed"] += 1 return None magic, sequence, sent_ns, payload_len = OMNI_HEADER.unpack_from(packet) payload = packet[OMNI_HEADER.size :] if magic != OMNI_MAGIC or payload_len != len(payload) or not payload: with self._lock: self._metrics["dropped_malformed"] += 1 return None raw_age_ms = (now_ns - sent_ns) / 1_000_000.0 effective_age_ms = ( 0.0 if baseline_ms is None else max(0.0, raw_age_ms - baseline_ms) ) return sequence, payload, raw_age_ms, effective_age_ms def _record_packet_age( self, raw_age_ms: float, baseline_ms: float, effective_age_ms: float ) -> None: with self._lock: self._metrics["raw_packet_age_ms"] = round(raw_age_ms, 3) self._metrics["packet_age_baseline_ms"] = round(baseline_ms, 3) self._metrics["effective_packet_age_ms"] = round(effective_age_ms, 3) previous_max = self._metrics["max_effective_packet_age_ms"] if previous_max is None or effective_age_ms > previous_max: self._metrics["max_effective_packet_age_ms"] = round( effective_age_ms, 3 ) @staticmethod def _validate_shape(data: dict[str, Any]) -> None: position = data["arm"]["position"] left = position["left"] right = position["right"] if len(left) != 7 or len(right) != 7: raise ValueError("arm.position must contain left[7] and right[7]") if not all(math.isfinite(float(v)) for v in [*left, *right]): raise ValueError("arm.position contains a non-finite value") class LocalTeleopBridge(Node): FRAME_ID = "tg3_local_teleop" def __init__(self, config: dict[str, Any], allow_publish: bool, status_file: Path) -> None: super().__init__("tg3_local_teleop") self.cfg = config self.allow_publish = allow_publish self.status_file = status_file ros_cfg = config["ros"] net_cfg = config["network"] self.hands_cfg = config.get("hands", {}) self.hands_enabled = bool(self.hands_cfg.get("enabled", False)) self.right_point_gesture_enabled = self.hands_enabled and bool( self.hands_cfg.get("right_b_point_gesture_enabled", False) ) self.right_point_gesture = GestureToggle( hold_seconds=float( self.hands_cfg.get("right_b_point_gesture_hold_seconds", 1.0) ), release_seconds=float( self.hands_cfg.get("right_b_point_gesture_release_seconds", 0.5) ), ) point_pose = self.hands_cfg.get( "right_b_point_pose_normalized", [0.2, 0.688, 0.0, 0.98, 0.98, 0.98], ) self.right_point_gesture_pose = [float(value) for value in point_pose] self.right_point_gesture_target = normalized_pose_to_positions( self.right_point_gesture_pose, int(self.hands_cfg.get("position_min", 1)), int(self.hands_cfg.get("position_max", 1000)), ) self.right_a_pose_enabled = self.hands_enabled and bool( self.hands_cfg.get("right_a_pose_enabled", False) ) self.right_a_pose = GuardedMomentaryGesture( release_seconds=float( self.hands_cfg.get("right_a_pose_release_seconds", 0.5) ) ) right_a_target = self.hands_cfg.get( "right_a_pose_positions", [428, 735, 500, 77, 77, 72] ) self.right_a_pose_target = [int(value) for value in right_a_target] hand_position_min = int(self.hands_cfg.get("position_min", 1)) hand_position_max = int(self.hands_cfg.get("position_max", 1000)) if len(self.right_a_pose_target) != 6 or not all( hand_position_min <= value <= hand_position_max for value in self.right_a_pose_target ): raise ValueError( "hands.right_a_pose_positions must contain 6 values within " "the configured BrainCo position range" ) self.locomotion_cfg = config.get("locomotion", {}) self.locomotion_enabled = bool( self.locomotion_cfg.get("enabled", False) ) self.head_cfg = config.get("head", {}) self.head_enabled = bool(self.head_cfg.get("enabled", False)) self.head_pitch_controller = HeadPitchController( deadzone=float(self.head_cfg.get("joystick_deadzone", 0.1)), max_speed_rad_s=float( self.head_cfg.get("max_pitch_speed_rad_s", 0.2) ), max_accel_rad_s2=float( self.head_cfg.get("max_pitch_accel_rad_s2", 0.5) ), min_pitch_rad=float( self.head_cfg.get("min_pitch_rad", -0.1745329252) ), max_pitch_rad=float( self.head_cfg.get("max_pitch_rad", 0.872664626) ), axis_sign=float(self.head_cfg.get("pitch_axis_sign", -1.0)), max_dt_s=float(self.head_cfg.get("max_integration_dt_s", 0.1)), ) self.data_collection_cfg = config.get("data_collection", {}) self.data_collection_enabled = bool( self.data_collection_cfg.get("enabled", False) ) self.data_collection_gate = RecordingToggleGate( hold_seconds=float( self.data_collection_cfg.get("button_hold_seconds", 1.0) ), release_seconds=float( self.data_collection_cfg.get("button_release_seconds", 0.5) ), ) self.data_control_publisher = None self.data_iarm_publisher = None self.data_recorder_status: dict[str, Any] = {} self.data_recorder_status_at = 0.0 self.data_pending_control: dict[str, Any] | None = None self.data_pending_control_since = 0.0 self.data_last_control_publish_at = 0.0 self.data_last_heartbeat_at = 0.0 self.data_last_iarm_received_at = 0.0 self.data_capture_id: str | None = None self.data_capture_session_id: str | None = None self.data_event_seq = 0 self.data_toggle_count = 0 self.data_last_transition = "initialized; waiting for an armed session" self.hand_publishers: dict[str, Any] = {} self.robot_hand_positions: dict[str, list[int] | None] = { side: None for side in HAND_SIDES } self.robot_hand_states: dict[str, list[int]] = { side: [] for side in HAND_SIDES } self.robot_hand_at: dict[str, float] = {side: 0.0 for side in HAND_SIDES} self.last_hand_commands: dict[str, list[int] | None] = { side: None for side in HAND_SIDES } self.expected_hand_messages: dict[ str, tuple[tuple[Any, ...], float] | None ] = {side: None for side in HAND_SIDES} self.last_hand_publish_at = 0.0 self.hand_publish_count = 0 self.hand_output_ready = False self.runtime_hand_output_reasons: list[str] = [] self.foreign_hand_source_seen = False self.walk_active = False self.walk_command = [0.0, 0.0] self.walk_publish_count = 0 self.walk_zero_frames_remaining = 0 self.walk_require_neutral = True self.last_walk_publish_at = 0.0 self.head_publisher = None self.robot_head_positions: dict[int, float] = {} self.robot_head_errors: dict[int, int] = {} self.robot_head_at = 0.0 self.expected_head_messages: list[tuple[tuple[Any, ...], float]] = [] self.foreign_head_command_at = 0.0 self.foreign_head_command_count = 0 self.head_publish_count = 0 self.last_head_publish_at = 0.0 self.head_output_reasons: list[str] = [] self.head_z_pressed: bool | None = None self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10) self.walk_publisher = None if self.locomotion_enabled: self.walk_publisher = self.create_publisher( TwistStamped, self.locomotion_cfg["command_topic"], 10 ) if self.head_enabled: self.head_publisher = self.create_publisher( HeadCtrl, self.head_cfg["command_topic"], 10 ) self.create_subscription( RobotState, self.head_cfg["state_topic"], self._on_robot_state, 10, ) self.create_subscription( HeadCtrl, self.head_cfg["command_topic"], self._on_head_command, 10, ) self.create_subscription( DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10 ) self.create_subscription( ArmStatus, ros_cfg["arm_state_topic"], self._on_arm_state, 10 ) # A foreign sample means cloud teleoperation is active. Never mix two # command sources on the vendor frequency-conversion input. self.create_subscription( JointState, ros_cfg["command_topic"], self._on_command_topic, 10 ) if self.hands_enabled: for side in HAND_SIDES: command_topic = str(self.hands_cfg[f"{side}_command_topic"]) status_topic = str(self.hands_cfg[f"{side}_status_topic"]) self.hand_publishers[side] = self.create_publisher( SetMotorMulti, command_topic, 10 ) self.create_subscription( MotorStatus, status_topic, lambda msg, hand_side=side: self._on_hand_status(hand_side, msg), 10, ) self.create_subscription( SetMotorMulti, command_topic, lambda msg, hand_side=side: self._on_hand_command(hand_side, msg), 10, ) self.create_service(Trigger, ros_cfg["home_service"], self._on_home_request) self.create_service( Trigger, ros_cfg["cancel_home_service"], self._on_cancel_home_request ) if getattr(self, "data_collection_enabled", False): self.data_control_publisher = self.create_publisher( String, str(self.data_collection_cfg["control_topic"]), 10, ) self.data_iarm_publisher = self.create_publisher( String, str(self.data_collection_cfg["iarm_frame_topic"]), 10, ) self.create_subscription( String, str(self.data_collection_cfg["status_topic"]), self._on_data_recorder_status, 10, ) self.source = LatestArmData(net_cfg) self.source.start() self.rl_state: dict[str, str] = {} self.rl_state_at = 0.0 self.robot_arm_positions: list[float] | None = None self.robot_arm_errors: list[int] = [] self.robot_arm_at = 0.0 self.foreign_source_seen = False self.armed = False self.returning_home = False self.home_command: list[float] | None = None self.home_started_at = 0.0 self.home_settle_started_at: float | None = None self.home_status = "idle" self.combo_started_at: float | None = None self.combo_latched = False self.active_session_id: str | None = None self.session_start_attempted_id: str | None = None self.last_session_state = "inactive" self.last_session_stop_reason = "" self.last_command: list[float] | None = None self.last_publish_at = 0.0 self.last_status_at = 0.0 self.last_log_signature: tuple[Any, ...] | None = None self.started_at = time.monotonic() rate = float(ros_cfg["publish_rate_hz"]) self.timer = self.create_timer(1.0 / rate, self._tick) mode = "ACTIVE-CAPABLE" if allow_publish else "MONITOR-ONLY" self.get_logger().info( f"local bridge started in {mode}; source={self.source.description}; " f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; " f"locomotion={self.locomotion_enabled}; " f"head_pitch={self.head_enabled}; " f"data_collection={self.data_collection_enabled}" ) def close(self) -> None: self._disarm("bridge is shutting down") self._finish_home("bridge is shutting down", success=False) self.source.close() def _on_rl_state(self, msg: DiagnosticStatus) -> None: self.rl_state = {item.key: item.value for item in msg.values} self.rl_state_at = time.monotonic() def _on_arm_state(self, msg: ArmStatus) -> None: by_id = {int(motor.name): motor for motor in msg.status} if not all(motor_id in by_id for motor_id in MOTOR_IDS): return positions = [float(by_id[motor_id].pos) for motor_id in MOTOR_IDS] if not all(math.isfinite(value) for value in positions): return self.robot_arm_positions = positions self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS] self.robot_arm_at = time.monotonic() def _on_robot_state(self, msg: RobotState) -> None: by_id = {int(motor.name): motor for motor in msg.head.status} pitch_id = int(self.head_cfg.get("pitch_motor_id", 2)) if pitch_id not in by_id: return positions = {motor_id: float(motor.pos) for motor_id, motor in by_id.items()} if not all(math.isfinite(value) for value in positions.values()): return self.robot_head_positions = positions self.robot_head_errors = { motor_id: int(motor.error) for motor_id, motor in by_id.items() } self.robot_head_at = time.monotonic() def _on_hand_status(self, side: str, msg: MotorStatus) -> None: positions = [int(value) for value in msg.positions] states = [int(value) for value in msg.states] if len(positions) != 6 or len(states) != 6: return self.robot_hand_positions[side] = positions self.robot_hand_states[side] = states self.robot_hand_at[side] = time.monotonic() def _on_data_recorder_status(self, msg: String) -> None: """Consume recorder acknowledgements without affecting robot control.""" try: status = json.loads(msg.data) if not isinstance(status, dict) or status.get("version") != 1: raise ValueError("unsupported recorder status") except (TypeError, ValueError, json.JSONDecodeError) as exc: self.get_logger().warning(f"ignored invalid data-recorder status: {exc}") return self.data_recorder_status = status self.data_recorder_status_at = time.monotonic() pending = self.data_pending_control if ( pending is not None and status.get("ack_request_id") == pending.get("request_id") and status.get("ack_event_seq") == pending.get("event_seq") ): accepted = status.get("ack_accepted") if type(accepted) is bool: self.data_pending_control = None self.data_pending_control_since = 0.0 if not accepted: detail = str( status.get("last_error") or status.get("ack_code") or "recorder rejected request" ) if pending.get("command") == "start": self.data_collection_gate.force_inactive( f"recorder_rejected: {detail}" ) self.data_last_transition = ( f"{pending.get('command')}_rejected: {detail}" ) self.get_logger().error( "data recorder rejected %s: %s" % (pending.get("command"), detail) ) state = status.get("state") if ( state in ("ready", "failed") and status.get("capture_id") == self.data_capture_id and self.data_collection_gate.active ): self.data_collection_gate.force_inactive( f"recorder_{state}: {status.get('last_error', '')}".rstrip() ) self.data_last_transition = self.data_collection_gate.last_transition elif ( self.data_pending_control is None and self.data_collection_gate.active and state in ("idle", "ready", "failed", "recording", "stopping") and status.get("capture_id") != self.data_capture_id ): # A restarted supervisor has no in-memory context for the old bag. # Reconcile the UI gate instead of displaying a false recording # state forever; the supervisor owns cleanup of its old cgroup and # active/failed directory. self.data_collection_gate.force_inactive( "recorder context was lost or replaced" ) self.data_last_transition = self.data_collection_gate.last_transition self.get_logger().error( "data recorder no longer owns the requested capture; " "release L3 before starting a new episode" ) def _publish_data_control(self, payload: dict[str, Any]) -> None: if self.data_control_publisher is None: return try: message = String() message.data = json.dumps( payload, ensure_ascii=False, separators=(",", ":") ) self.data_control_publisher.publish(message) self.data_last_control_publish_at = time.monotonic() except Exception as exc: # Data collection must never stop robot control. self.get_logger().error(f"cannot publish data-recorder control: {exc}") def _request_data_capture(self, command: str, reason: str) -> None: if not self.data_collection_enabled: return if command == "start": self.data_capture_id = ( time.strftime("%Y%m%dT%H%M%SZ", time.gmtime()) + "_TG3_" + uuid.uuid4().hex[:8] ) if self.data_capture_id is None: return if self.data_capture_session_id is None: self.data_capture_session_id = self.active_session_id or ( "direct_" + uuid.uuid4().hex ) self.data_event_seq += 1 payload: dict[str, Any] = { "version": 1, "command": command, "event_seq": self.data_event_seq, "request_id": uuid.uuid4().hex, "capture_id": self.data_capture_id, "teleop_session_id": self.data_capture_session_id, "reason": reason, "sent_unix_s": time.time(), } self.data_pending_control = payload self.data_pending_control_since = time.monotonic() self._publish_data_control(payload) self.data_last_transition = f"{command}_requested: {reason}" def _publish_data_heartbeat(self) -> None: if self.data_capture_id is None or self.data_capture_session_id is None: return self.data_event_seq += 1 payload = { "version": 1, "command": "heartbeat", "event_seq": self.data_event_seq, "request_id": uuid.uuid4().hex, "capture_id": self.data_capture_id, "teleop_session_id": self.data_capture_session_id, "sent_unix_s": time.time(), } self._publish_data_control(payload) self.data_last_heartbeat_at = time.monotonic() def _tick_data_collection( self, now: float, sample: ArmSnapshot | None ) -> None: if not self.data_collection_enabled: return if self.armed: input_timeout = float( self.data_collection_cfg.get("button_input_timeout_s", 0.25) ) input_healthy = ( sample is not None and now - sample.received_at <= input_timeout ) pressed = ( None if sample is None else left_joystick_pressed(sample.data) ) action = self.data_collection_gate.update( now, input_healthy=input_healthy, pressed=pressed, ) if action is not None: self.data_toggle_count += 1 self._request_data_capture( action, "left joystick press held for " f"{self.data_collection_gate.hold_seconds:.1f}s", ) retry_s = float(self.data_collection_cfg.get("control_retry_seconds", 0.5)) if ( self.data_pending_control is not None and now - self.data_last_control_publish_at >= retry_s ): self._publish_data_control(self.data_pending_control) ack_timeout_s = float( self.data_collection_cfg.get("ack_timeout_seconds", 5.0) ) if ( self.data_pending_control is not None and self.data_pending_control_since > 0.0 and now - self.data_pending_control_since >= ack_timeout_s ): expired_command = str(self.data_pending_control.get("command")) self.data_pending_control = None self.data_pending_control_since = 0.0 if expired_command == "start": self.data_collection_gate.force_inactive( "recorder START acknowledgement timed out" ) self.data_last_transition = f"{expired_command}_ack_timeout" self.get_logger().error( f"data recorder {expired_command} acknowledgement timed out" ) status_stale_s = float( self.data_collection_cfg.get("status_stale_seconds", 4.0) ) if ( self.data_collection_gate.active and self.data_pending_control is None and self.data_recorder_status_at > 0.0 and now - self.data_recorder_status_at >= status_stale_s ): self.data_collection_gate.force_inactive( "recorder status heartbeat became stale" ) self.data_last_transition = "recorder_status_stale" self._request_data_capture( "stop", "recorder status heartbeat became stale" ) self.get_logger().error( "data recorder status became stale; capture stop requested" ) recorder_state = self.data_recorder_status.get("state") heartbeat_s = float( self.data_collection_cfg.get("heartbeat_interval_seconds", 0.5) ) if ( self.data_collection_gate.active and self.data_pending_control is None and recorder_state in ("starting", "recording") and now - self.data_last_heartbeat_at >= heartbeat_s ): self._publish_data_heartbeat() if ( self.data_collection_gate.active and sample is not None and sample.received_at != self.data_last_iarm_received_at and self.data_iarm_publisher is not None ): try: message = String() # Preserve the complete xTELE/OmniSocket application frame. message.data = json.dumps( sample.data, ensure_ascii=False, separators=(",", ":") ) self.data_iarm_publisher.publish(message) self.data_last_iarm_received_at = sample.received_at except Exception as exc: self.get_logger().error(f"cannot publish xTELE capture frame: {exc}") @staticmethod def _hand_message_signature(msg: SetMotorMulti) -> tuple[Any, ...]: return ( int(msg.mode), tuple(int(value) for value in msg.positions), tuple(int(value) for value in msg.speeds), tuple(int(value) for value in msg.currents), tuple(int(value) for value in msg.pwms), tuple(int(value) for value in msg.durations), ) def _on_hand_command(self, side: str, msg: SetMotorMulti) -> None: now = time.monotonic() signature = self._hand_message_signature(msg) expected = self.expected_hand_messages[side] if ( expected is not None and now - expected[1] <= 0.25 and signature == expected[0] ): return if not self.foreign_hand_source_seen: self.get_logger().error( f"foreign {side} BrainCo hand command detected; local control is locked " "until this bridge is restarted" ) self.foreign_hand_source_seen = True if self.armed: self._disarm("foreign BrainCo hand command source detected") if self.returning_home: self._finish_home( "foreign BrainCo hand command source detected", success=False ) @staticmethod def _head_message_signature(msg: HeadCtrl) -> tuple[Any, ...]: return ( int(msg.mode), int(msg.label), int(msg.reserved), tuple( ( int(motor.name), float(motor.kp), float(motor.kd), float(motor.pos), float(motor.spd), float(motor.tor), float(motor.cur), str(motor.joint_ids), ) for motor in msg.ctrl ), ) def _on_head_command(self, msg: HeadCtrl) -> None: now = time.monotonic() window = float(self.head_cfg.get("self_command_window_s", 0.25)) self.expected_head_messages = [ expected for expected in self.expected_head_messages if now - expected[1] <= window ] signature = self._head_message_signature(msg) if any(signature == expected[0] for expected in self.expected_head_messages): return self.foreign_head_command_at = now self.foreign_head_command_count += 1 if self.head_pitch_controller.active: self.get_logger().warning( "external /head/cmd detected; local head-pitch output paused" ) def _on_command_topic(self, msg: JointState) -> None: if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16): if not self.foreign_source_seen: self.get_logger().error( "foreign /encoder_identical_joint data detected; local control is locked " "until this bridge is restarted" ) self.foreign_source_seen = True if self.armed: self._disarm("foreign/cloud arm command source detected") if self.returning_home: self._finish_home("foreign/cloud arm command source detected", success=False) def _on_home_request( self, _request: Trigger.Request, response: Trigger.Response ) -> Trigger.Response: now = time.monotonic() sample, source_error = self.source.get() reasons = self._home_start_reasons(now, sample, source_error) if self.armed: reasons.append("manual isomorphic-arm control is armed") if reasons: response.success = False response.message = "return-home rejected: " + "; ".join(reasons) return response self._start_home(now, "operator return-home service") response.success = True response.message = ( "limited-speed return-home accepted; use cancel service or hold left Z + " "right C for 3s to stop" ) return response def _on_cancel_home_request( self, _request: Trigger.Request, response: Trigger.Response ) -> Trigger.Response: if not self.returning_home: response.success = False response.message = "return-home is not running" return response self._finish_home("operator cancel service", success=False, cancelled=True) response.success = True response.message = "return-home cancelled; arm command publication stopped" return response def _tick(self) -> None: now = time.monotonic() sample, source_error = self.source.get() if self.source.transport == "omnisocket": self._update_session_gate(now, sample, source_error) else: # Direct-LAN fallback retains the original local raw-button latch. self._update_combo(now, sample) # Custom joint-target limits are checked only when arming below. Once # capture/following starts, the vendor node keeps its own unchanged # limit protection while this bridge continues the non-limit gates. startup_reasons = self._safety_reasons(now, sample, source_error) armed_runtime_reasons = self._safety_reasons( now, sample, source_error, check_iarm_target=False, check_hands=self.armed, # Hand feedback is not an all-teleop teardown gate. A separate # runtime gate below pauses only hand publication during a status # gap. Foreign publishers and command-shape validation remain # active through check_hands. check_hand_feedback=False, ) home_runtime_reasons = self._safety_reasons( now, sample, source_error, check_iarm=False, check_iarm_target=False, check_hands=False, check_hand_feedback=False, ) runtime_hand_reasons = ( self._hand_feedback_reasons(now) if self.hands_enabled and self.armed else [] ) self.runtime_hand_output_reasons = runtime_hand_reasons if self.armed and armed_runtime_reasons: self._disarm( "runtime safety gate failed: " + "; ".join(armed_runtime_reasons) ) if self.returning_home and home_runtime_reasons: self._finish_home( "runtime safety gate failed: " + "; ".join(home_runtime_reasons), success=False, ) # The recorder toggle is deliberately downstream of every disarm # decision, so a matching STOP or safety teardown always wins over an # L3 press observed in the same control tick. self._tick_data_collection(now, sample) if self.head_enabled: self._tick_head(now, sample) if self.right_point_gesture_enabled: gesture_toggled = self.right_point_gesture.update( now, armed=self.armed, input_healthy=( self.armed and sample is not None and not runtime_hand_reasons ), pressed=( None if sample is None else right_b_pressed(sample.data) ), ) if gesture_toggled: state = "ACTIVE" if self.right_point_gesture.active else "INACTIVE" self.get_logger().warning( f"BILATERAL POINT GESTURE {state}: right B held for " f"{self.right_point_gesture.hold_seconds:.1f}s" ) if self.right_a_pose_enabled: pose_changed = self.right_a_pose.update( now, armed=self.armed, input_healthy=( self.armed and sample is not None and not runtime_hand_reasons ), pressed=( None if sample is None else right_a_pressed(sample.data) ), ) if pose_changed: state = "ACTIVE" if self.right_a_pose.active else "INACTIVE" self.get_logger().warning( f"RIGHT-HAND A POSE {state}: momentary right A" ) if self.returning_home: self._tick_home(now) elif self.armed and sample is not None: desired = self._positions(sample.data) command = self._slew_limit(desired, now) self._publish_target(command, now) self.last_command = command if self.hands_enabled: if runtime_hand_reasons: if self.hand_output_ready: self.get_logger().warning( "BRAINCO HAND OUTPUT PAUSED: " + "; ".join(runtime_hand_reasons) ) self.hand_output_ready = False else: if not self.hand_output_ready: # Resume the hand slew limiter at measured feedback, # never at the last target from before the status gap. self.last_hand_commands = { side: list(self.robot_hand_positions[side] or []) for side in HAND_SIDES } self.last_hand_publish_at = now self.get_logger().info( "BRAINCO HAND OUTPUT READY: feedback healthy" ) self.hand_output_ready = True desired_hands = self._hand_targets(sample.data) if self.right_point_gesture_enabled: for side in HAND_SIDES: freeze_reference = self.last_hand_commands[side] if ( freeze_reference is None or len(freeze_reference) != 6 ): freeze_reference = self.robot_hand_positions[side] desired_hands[side] = select_hand_target( desired_hands[side], freeze_reference, self.right_point_gesture_target, active=self.right_point_gesture.active, freeze=self.right_point_gesture.freeze_right_hand, ) if self.right_a_pose_enabled: freeze_reference = self.last_hand_commands["right"] if freeze_reference is None or len(freeze_reference) != 6: freeze_reference = self.robot_hand_positions["right"] # Right A has momentary priority over the persistent B # bilateral gesture on the right hand; releasing A # returns through the same slew limiter to B or live # xTELE input. The left B target remains unchanged. desired_hands["right"] = select_hand_target( desired_hands["right"], freeze_reference, self.right_a_pose_target, active=self.right_a_pose.active, freeze=self.right_a_pose.freeze_right_hand, ) commands = { side: self._slew_hand(side, desired_hands[side], now) for side in HAND_SIDES } self._publish_hands(commands, now) self.last_hand_commands = commands if self.locomotion_enabled: self._tick_locomotion(now, sample) elif self.locomotion_enabled: self._tick_walk_zero_burst(now) if self.returning_home: reasons = home_runtime_reasons elif self.armed: reasons = armed_runtime_reasons else: reasons = startup_reasons if now - self.last_status_at >= 0.2: self._write_status(now, sample, reasons) self.last_status_at = now signature = ( self.armed, self.returning_home, bool(sample), tuple(reasons), self.rl_state.get("current_state"), self.rl_state.get("status"), ) if signature != self.last_log_signature: if self.returning_home: state = "RETURNING_HOME" else: state = "ARMED" if self.armed else "DISARMED" detail = "all safety gates ready" if not reasons else "; ".join(reasons) # rclpy associates severity with the Python call site, therefore # INFO and WARN need separate call sites instead of a bound method. if reasons: self.get_logger().warning(f"{state}: {detail}") else: self.get_logger().info(f"{state}: {detail}") self.last_log_signature = signature restart_reason = self._source_restart_reason(now, sample) if restart_reason is not None: # A server outage can leave the native OmniSocket recv call stuck in # an apparently connected session. Raising out of the ROS loop lets # systemd replace the whole process (and native session) cleanly. self.get_logger().error(restart_reason) raise RuntimeError(restart_reason) def _source_restart_reason( self, now: float, sample: ArmSnapshot | None ) -> str | None: net_cfg = self.cfg["network"] if str(net_cfg.get("transport", "zmq")).lower() != "omnisocket": return None restart_after = float( net_cfg.get("omnisocket_restart_after_stale_s", 0.0) ) if restart_after <= 0.0: return None # No xTELE business frame before START, and no frame after a matching # STOP, are both normal idle states. Only a session that was last seen # as START/ACTIVE may use business-frame staleness to rebuild a stuck # native OmniSocket receiver. if sample is None: return None info = self._teleop_session_info(sample.data) if info is not None and info[2] == "stop": return None stale_for = now - sample.received_at if stale_for < restart_after: return None return ( f"OmniSocket input stale for {stale_for:.3f}s; exiting so systemd " "can establish a fresh session" ) @staticmethod def _teleop_session_info( data: dict[str, Any] ) -> tuple[str, int, str, str] | None: metadata = data.get("tg3_transport") if not isinstance(metadata, dict): return None version = metadata.get("protocol_version") session_id = metadata.get("session_id") session_seq = metadata.get("session_seq") session_state = metadata.get("session_state") stop_reason = metadata.get("stop_reason", "") if version != TELEOP_PROTOCOL_VERSION: return None if not ( isinstance(session_id, str) and len(session_id) == 32 and all(character in "0123456789abcdef" for character in session_id) ): return None if ( isinstance(session_seq, bool) or not isinstance(session_seq, int) or session_seq <= 0 ): return None if session_state not in ("start", "active", "stop"): return None if not isinstance(stop_reason, str): return None return session_id, session_seq, session_state, stop_reason def _update_session_gate( self, now: float, sample: ArmSnapshot | None, source_error: str, ) -> None: if sample is None: return info = self._teleop_session_info(sample.data) if info is None: self.last_session_state = "invalid" if self.armed: self._disarm("missing or invalid teleoperation session metadata") self.active_session_id = None return session_id, _session_seq, session_state, stop_reason = info self.last_session_state = session_state self.last_session_stop_reason = stop_reason if session_state == "active": if self.armed and session_id != self.active_session_id: self._disarm("teleoperation session ID changed without START") self.active_session_id = None return if session_state == "stop": if session_id != self.active_session_id: return was_armed = self.armed self._disarm("matching teleoperation STOP received") self.active_session_id = None if ( was_armed and stop_reason == "operator" and bool(self.cfg["control"].get("auto_home_on_stop", True)) ): self._start_home_if_safe( now, sample, source_error, "operator teleoperation STOP", ) return # START is repeated for a short bounded window so the receiver's # latest-frame conflation cannot hide the only arming event. A given # session is attempted exactly once; a rejected or interrupted session # cannot arm later without a new physical Z+C cycle and new ID. if session_id == self.active_session_id and self.armed: return if session_id == self.session_start_attempted_id: return self.session_start_attempted_id = session_id if self.armed: self._disarm("new START arrived while another session was armed") self.active_session_id = None return if not self.allow_publish: self.get_logger().warning( "teleoperation START received, but this process is MONITOR-ONLY" ) return reasons = self._safety_reasons(now, sample, source_error) if reasons: self.get_logger().error("cannot arm session: " + "; ".join(reasons)) return if self.returning_home: self._finish_home( "new teleoperation START accepted", success=False, cancelled=True ) self.active_session_id = session_id if self.right_point_gesture_enabled: self.right_point_gesture.new_session() if self.right_a_pose_enabled: self.right_a_pose.new_session() if self.head_enabled: self.head_pitch_controller.new_session() if getattr(self, "data_collection_enabled", False): self.data_capture_session_id = session_id self.data_collection_gate.new_session() self.data_last_transition = "new teleoperation session" self.armed = True self.walk_require_neutral = True # Start both slew limiters at measured robot feedback, never at a # potentially distant first network target. self.last_command = list(self.robot_arm_positions or []) self.last_publish_at = now if self.hands_enabled: self.last_hand_commands = { side: list(self.robot_hand_positions[side] or []) for side in HAND_SIDES } self.last_hand_publish_at = now self.get_logger().warning( "LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED by validated EAI " "teleoperation session; vendor drivers remain active" ) def _update_combo(self, now: float, sample: ArmSnapshot | None) -> None: pressed = False if sample is not None: buttons = sample.data.get("button", {}) left = buttons.get("left", []) right = buttons.get("right", []) # TS1P raw order is left X/Y/Z/... and right A/B/C/.... pressed = len(left) >= 3 and len(right) >= 3 and bool(left[2]) and bool(right[2]) if not pressed: self.combo_started_at = None self.combo_latched = False return if self.combo_started_at is None: self.combo_started_at = now hold_seconds = float(self.cfg["control"]["start_stop_hold_seconds"]) if self.combo_latched or now - self.combo_started_at < hold_seconds: return self.combo_latched = True if self.returning_home: self._finish_home( "left Z + right C held: operator cancel", success=False, cancelled=True ) return if self.armed: self._disarm("left Z + right C held: teleoperation ended") if bool(self.cfg["control"].get("auto_home_on_stop", True)): snapshot, source_error = self.source.get() self._start_home_if_safe( now, snapshot, source_error, "teleoperation ended by left Z + right C", ) return if not self.allow_publish: self.get_logger().warning( "left Z + right C held, but this process is MONITOR-ONLY" ) return snapshot, source_error = self.source.get() reasons = self._safety_reasons(now, snapshot, source_error) if reasons: self.get_logger().error("cannot arm: " + "; ".join(reasons)) return if self.right_point_gesture_enabled: self.right_point_gesture.new_session() if self.right_a_pose_enabled: self.right_a_pose.new_session() if self.head_enabled: self.head_pitch_controller.new_session() if getattr(self, "data_collection_enabled", False): self.data_capture_session_id = "direct_" + uuid.uuid4().hex self.data_collection_gate.new_session() self.data_last_transition = "new direct-LAN teleoperation session" self.armed = True self.walk_require_neutral = True # Start the slew limiter at measured robot feedback. Using None here # would make the first armed frame jump directly to the TS1P target. self.last_command = list(self.robot_arm_positions or []) self.last_publish_at = now if self.hands_enabled: self.last_hand_commands = { side: list(self.robot_hand_positions[side] or []) for side in HAND_SIDES } self.last_hand_publish_at = now self.get_logger().warning( "LOCAL DUAL-ARM + BRAINCO HAND CONTROL ARMED; vendor arm and hand " "drivers remain active" ) def _disarm(self, reason: str) -> None: was_armed = self.armed if getattr(self, "data_collection_enabled", False): action = self.data_collection_gate.end_session(reason) self.data_last_transition = self.data_collection_gate.last_transition if action == "stop": # This is a non-blocking ROS request. It never delays robot # disarm, locomotion zeroing, or the existing Home sequence. self._request_data_capture("stop", reason) self._stop_locomotion(reason) if self.right_point_gesture_enabled: # Clearing the logical override must not publish a hand target. # Existing STOP behavior leaves the physical hand at its last # limited command until a later, newly armed session. self.right_point_gesture.disarm(reason) if self.right_a_pose_enabled: self.right_a_pose.disarm(reason) if self.head_enabled: self.head_pitch_controller.disarm(reason) self.armed = False self.hand_output_ready = False self.runtime_hand_output_reasons = [] self.last_command = None self.last_hand_commands = {side: None for side in HAND_SIDES} if was_armed: self.get_logger().warning( f"LOCAL DUAL-ARM + BRAINCO HAND CONTROL DISARMED: {reason}" ) @staticmethod def _shape_joystick_axis(value: float, deadzone: float, expo: float) -> float: """Apply the xTELE deadzone and a normalized exponential response.""" if not math.isfinite(value): raise ValueError("joystick axis is non-finite") if not 0.0 <= deadzone < 1.0: raise ValueError("joystick deadzone must be in [0, 1)") if not math.isfinite(expo) or expo <= 0.0: raise ValueError("joystick exponent must be positive") value = max(-1.0, min(1.0, value)) magnitude = abs(value) if magnitude <= deadzone: return 0.0 normalized = (magnitude - deadzone) / (1.0 - deadzone) return math.copysign(normalized**expo, value) def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None: cfg = self.locomotion_cfg try: left_joystick = sample.data["joystick"]["left"] right_joystick = sample.data["joystick"]["right"] if len(left_joystick) != 2 or len(right_joystick) != 2: raise ValueError("left and right joysticks must each contain two axes") # xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal]. # Use the left vertical axis for translation and the right # horizontal axis for in-place yaw, matching the physical control # convention requested for this installation. raw_forward = float(left_joystick[0]) raw_yaw = float(right_joystick[1]) head_mode = left_z_pressed(sample.data) if head_mode is None: raise ValueError("left Z state is malformed") deadzone = float(cfg["joystick_deadzone"]) forward_expo = float(cfg["joystick_expo"]) yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo)) shaped_forward = self._shape_joystick_axis( raw_forward, deadzone, forward_expo ) shaped_yaw = self._shape_joystick_axis( raw_yaw, deadzone, yaw_expo ) # Left Z reserves the entire right stick for head-pitch mode. # Suppress yaw so a small sideways component while looking up or # down cannot turn the robot. if head_mode: shaped_yaw = 0.0 signed_forward = shaped_forward * float( cfg.get("forward_axis_sign", 1.0) ) max_forward = float(cfg["max_forward_m_s"]) max_reverse = float(cfg["max_reverse_m_s"]) max_angular = float(cfg["max_angular_rad_s"]) if not all( math.isfinite(value) and value >= 0.0 for value in (max_forward, max_reverse, max_angular) ): raise ValueError("locomotion limits must be finite and non-negative") except (KeyError, TypeError, ValueError) as exc: self._stop_locomotion(f"invalid locomotion input: {exc}") return forward_active = shaped_forward != 0.0 yaw_active = shaped_yaw != 0.0 if self.walk_require_neutral: if forward_active or yaw_active: self._stop_locomotion( "waiting for both locomotion sticks to return to neutral" ) self._tick_walk_zero_burst(now) return self.walk_require_neutral = False if not forward_active and not yaw_active: self._stop_locomotion("both locomotion stick axes are neutral") self._tick_walk_zero_burst(now) return if not self.walk_active: self.walk_active = True self.walk_zero_frames_remaining = 0 self.get_logger().warning( "LOCAL HBWALK VELOCITY STARTED: direct left-stick forward " "or right-stick yaw" ) linear_limit = max_forward if signed_forward >= 0.0 else max_reverse linear_x = signed_forward * linear_limit if forward_active else 0.0 angular_z = 0.0 if yaw_active: angular_z = ( shaped_yaw * float(cfg.get("yaw_axis_sign", -1.0)) * max_angular ) self._publish_walk(linear_x, angular_z, now) def _stop_locomotion(self, reason: str) -> None: was_active = self.walk_active had_nonzero_command = any(abs(value) > 1e-9 for value in self.walk_command) self.walk_active = False if was_active or had_nonzero_command: now = time.monotonic() self._publish_walk(0.0, 0.0, now) self.walk_zero_frames_remaining = max( 0, int(self.locomotion_cfg.get("zero_burst_frames", 1)) - 1 ) self.get_logger().warning(f"LOCAL HBWALK LOCOMOTION STOPPED: {reason}") def _tick_walk_zero_burst(self, now: float) -> None: if self.walk_zero_frames_remaining <= 0: return self._publish_walk(0.0, 0.0, now) self.walk_zero_frames_remaining -= 1 def _publish_walk(self, linear_x: float, angular_z: float, now: float) -> None: if not self.allow_publish or self.walk_publisher is None: return msg = TwistStamped() msg.header.stamp = self.get_clock().now().to_msg() # The TG3 secondary-development topic specification uses "pelvis". msg.header.frame_id = str(self.locomotion_cfg.get("frame_id", "")) msg.twist.linear.x = float(linear_x) msg.twist.linear.y = 0.0 msg.twist.linear.z = 0.0 msg.twist.angular.x = 0.0 msg.twist.angular.y = 0.0 msg.twist.angular.z = float(angular_z) self.walk_publisher.publish(msg) self.walk_command = [float(linear_x), float(angular_z)] self.last_walk_publish_at = now self.walk_publish_count += 1 def _tick_head(self, now: float, sample: ArmSnapshot | None) -> None: pitch_id = int(self.head_cfg.get("pitch_motor_id", 2)) pitch_position = self.robot_head_positions.get(pitch_id) feedback_timeout = float(self.head_cfg.get("feedback_timeout_s", 0.25)) feedback_age = ( math.inf if self.robot_head_at == 0.0 else now - self.robot_head_at ) feedback_healthy = ( pitch_position is not None and math.isfinite(float(pitch_position)) and feedback_age <= feedback_timeout and int(self.robot_head_errors.get(pitch_id, -1)) == 0 ) input_timeout = float(self.head_cfg.get("input_timeout_s", 0.3)) input_healthy = ( self.armed and sample is not None and now - sample.received_at <= input_timeout ) self.head_z_pressed = ( None if sample is None else left_z_pressed(sample.data) ) axis = None if sample is None else head_pitch_command_axis(sample.data) quiet_s = float(self.head_cfg.get("foreign_command_quiet_s", 1.0)) foreign_age = ( math.inf if self.foreign_head_command_at == 0.0 else now - self.foreign_head_command_at ) external_busy = foreign_age <= quiet_s reasons: list[str] = [] if not self.armed: reasons.append("teleoperation is not armed") if not input_healthy: reasons.append("right-stick input is unavailable/stale") if axis is None: reasons.append("left Z or right-stick vertical input is malformed") elif not self.head_z_pressed: reasons.append("left Z is not held; head-pitch control is idle") if not feedback_healthy: reasons.append("head pitch feedback is unavailable/stale or in error") if external_busy: reasons.append("external /head/cmd publisher has an active lease") self.head_output_reasons = reasons target = self.head_pitch_controller.update( now, armed=self.armed, input_healthy=input_healthy, feedback_healthy=feedback_healthy, external_busy=external_busy, axis=axis, feedback_position_rad=pitch_position, ) if target is not None: self._publish_head_pitch(target, now) def _publish_head_pitch(self, target_rad: float, now: float) -> None: if not self.allow_publish or self.head_publisher is None: return motor = MotorCtrl() motor.name = int(self.head_cfg.get("pitch_motor_id", 2)) motor.kp = 0.0 motor.kd = 0.0 motor.pos = float(target_rad) motor.spd = float(self.head_cfg.get("command_speed_rad_s", 0.2)) motor.tor = 0.0 motor.cur = float(self.head_cfg.get("max_current_a", 1.0)) motor.joint_ids = "" msg = HeadCtrl() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = str(self.head_cfg.get("frame_id", "head")) msg.mode = int(self.head_cfg.get("mode", 0)) msg.label = int(self.head_cfg.get("label", 151)) msg.reserved = 0 msg.ctrl = [motor] signature = self._head_message_signature(msg) window = float(self.head_cfg.get("self_command_window_s", 0.25)) self.expected_head_messages = [ expected for expected in self.expected_head_messages if now - expected[1] <= window ][-64:] self.expected_head_messages.append((signature, now)) self.head_publisher.publish(msg) self.head_publish_count += 1 self.last_head_publish_at = now def _home_start_reasons( self, now: float, sample: ArmSnapshot | None, source_error: str, ) -> list[str]: reasons = self._safety_reasons( now, sample, source_error, check_iarm=False, check_iarm_target=False, check_hands=False, ) if not self.allow_publish: reasons.append("bridge is monitor-only") if self.returning_home: reasons.append("return-home is already running") if self.robot_arm_positions is None: reasons.append("robot arm position is unavailable") reasons.extend(self._home_config_reasons()) return reasons def _start_home(self, now: float, reason: str) -> None: assert self.robot_arm_positions is not None self.returning_home = True self.home_command = list(self.robot_arm_positions) self.home_started_at = now self.home_settle_started_at = None self.home_status = "running" self.last_publish_at = now self.get_logger().warning( f"LIMITED-SPEED RETURN-HOME STARTED from measured robot arm state ({reason})" ) def _start_home_if_safe( self, now: float, sample: ArmSnapshot | None, source_error: str, reason: str, ) -> bool: reasons = self._home_start_reasons(now, sample, source_error) if reasons: detail = "; ".join(reasons) self.home_status = "failed: automatic return-home rejected: " + detail self.get_logger().error( f"AUTOMATIC RETURN-HOME REJECTED after {reason}: {detail}" ) return False self._start_home(now, reason) return True def _tick_home(self, now: float) -> None: if self.home_command is None or self.robot_arm_positions is None: self._finish_home("home trajectory state is unavailable", success=False) return home_cfg = self.cfg["home"] if now - self.home_started_at > float(home_cfg["timeout_s"]): self._finish_home("return-home timeout", success=False) return tracking_error = max( abs(commanded - measured) for commanded, measured in zip( self.home_command, self.robot_arm_positions ) ) if tracking_error > float(home_cfg["max_tracking_error_rad"]): self._finish_home( f"robot is not following the home trajectory " f"(max error {tracking_error:.3f} rad)", success=False, ) return dt = max(0.001, now - self.last_publish_at) max_step = float(home_cfg["slew_rad_s"]) * dt goal = [float(v) for v in home_cfg["joint_goal_rad"]] if len(goal) != 14: self._finish_home("configuration must define a 14-joint home goal", success=False) return command = [ current + max(-max_step, min(max_step, target - current)) for current, target in zip(self.home_command, goal) ] self._publish_target(command, now) self.home_command = command command_tolerance = float(home_cfg["command_tolerance_rad"]) actual_tolerance = float(home_cfg["actual_tolerance_rad"]) command_at_goal = all( abs(current - target) <= command_tolerance for current, target in zip(command, goal) ) robot_at_goal = all( abs(current - target) <= actual_tolerance for current, target in zip(self.robot_arm_positions, goal) ) if command_at_goal and robot_at_goal: if self.home_settle_started_at is None: self.home_settle_started_at = now elif now - self.home_settle_started_at >= float(home_cfg["settle_s"]): self._finish_home("both arms reached the configured home pose", success=True) else: self.home_settle_started_at = None def _finish_home( self, reason: str, success: bool, cancelled: bool = False ) -> None: was_running = self.returning_home self.returning_home = False self.home_command = None self.home_settle_started_at = None if not was_running: return if success: self.home_status = "complete" self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME COMPLETE: {reason}") elif cancelled: self.home_status = "cancelled: " + reason self.get_logger().warning(f"LIMITED-SPEED RETURN-HOME CANCELLED: {reason}") else: self.home_status = "failed: " + reason self.get_logger().error(f"LIMITED-SPEED RETURN-HOME FAILED: {reason}") def _publish_target(self, command: list[float], now: float) -> None: msg = JointState() msg.header.stamp = self.get_clock().now().to_msg() msg.header.frame_id = self.FRAME_ID msg.name = JOINT_NAMES msg.position = command # The vendor node checks both arrays for 14/16 DOF. Zero velocity # preserves its position-control path and avoids malformed-DOF warnings. msg.velocity = [0.0] * 14 self.publisher.publish(msg) self.last_publish_at = now def _publish_hands(self, commands: dict[str, list[int]], now: float) -> None: mode = int(self.hands_cfg["mode"]) duration = int(self.hands_cfg["duration_ms"]) for side in HAND_SIDES: msg = SetMotorMulti() msg.mode = mode msg.positions = commands[side] msg.speeds = [0] * 6 msg.currents = [0] * 6 msg.pwms = [0] * 6 msg.durations = [duration] * 6 signature = self._hand_message_signature(msg) self.expected_hand_messages[side] = (signature, now) self.hand_publishers[side].publish(msg) self.last_hand_publish_at = now self.hand_publish_count += 1 def _safety_reasons( self, now: float, sample: ArmSnapshot | None, source_error: str, check_iarm: bool = True, check_iarm_target: bool = True, check_hands: bool = True, check_hand_feedback: bool | None = None, ) -> list[str]: reasons: list[str] = [] net_cfg = self.cfg["network"] robot_cfg = self.cfg["robot"] control_cfg = self.cfg["control"] if check_hand_feedback is None: check_hand_feedback = check_hands if check_iarm and sample is None: reasons.append(source_error or "no isomorphic-arm data") elif check_iarm: data = sample.data expected_id = str(net_cfg.get("expected_iarm_id", "")) expected_type = str(net_cfg.get("expected_iarm_type", "")) if expected_id and data.get("isomorphic_arm_id") != expected_id: reasons.append("unexpected isomorphic-arm ID") if expected_type and data.get("isomorphic_arm_type") != expected_type: reasons.append("unexpected isomorphic-arm type") errors = data.get("servo_error", {}) arm_errors = [*errors.get("left", []), *errors.get("right", [])] if len(arm_errors) != 14 or any(int(v) != 0 for v in arm_errors): reasons.append("arm servo error is non-zero or incomplete") joy_errors = data.get("joycan_error", []) if len(joy_errors) < 2 or any(int(v) != 0 for v in joy_errors[:2]): reasons.append("TS1P controller/CAN error") minimum_hz = float(net_cfg["minimum_arm_frequency_hz"]) freq = data.get("freq", {}) if float(freq.get("left", 0.0)) < minimum_hz or float( freq.get("right", 0.0) ) < minimum_hz: reasons.append("TS1P arm sampling frequency too low") if check_iarm_target: reasons.extend(self._limit_reasons(self._positions(data), control_cfg)) if self.hands_enabled and check_hands: try: self._hand_targets(data) except (KeyError, TypeError, ValueError) as exc: reasons.append(f"invalid isomorphic-hand target: {exc}") arm_state_age = now - self.robot_arm_at if self.robot_arm_at == 0.0 or arm_state_age > float( robot_cfg["arm_state_timeout_s"] ): reasons.append("robot arm state unavailable/stale") elif len(self.robot_arm_errors) != 14 or any(self.robot_arm_errors): reasons.append("robot arm motor error is non-zero or incomplete") if self.hands_enabled and check_hand_feedback: reasons.extend(self._hand_feedback_reasons(now)) rl_age = now - self.rl_state_at if self.rl_state_at == 0.0 or rl_age > float(robot_cfg["state_timeout_s"]): reasons.append("robot RL state unavailable/stale") else: required = str(robot_cfg["required_state"]) if self.rl_state.get("current_state") != required: reasons.append(f"robot current_state is not {required}") if self.rl_state.get("child_state") != required: reasons.append(f"robot child_state is not {required}") if self.rl_state.get("status") != str(robot_cfg["required_status"]): reasons.append("robot RL status is not running") if self.foreign_source_seen: reasons.append("foreign/cloud arm command source was detected") if self.hands_enabled and check_hands and self.foreign_hand_source_seen: reasons.append("foreign BrainCo hand command source was detected") return reasons def _hand_feedback_reasons(self, now: float) -> list[str]: """Return reasons that should pause hand output, without stopping arms.""" reasons: list[str] = [] hand_timeout = float(self.hands_cfg["status_timeout_s"]) for side in HAND_SIDES: age = now - self.robot_hand_at[side] positions = self.robot_hand_positions[side] states = self.robot_hand_states[side] if self.robot_hand_at[side] == 0.0 or age > hand_timeout: reasons.append(f"robot {side} hand state unavailable/stale") elif positions is None or len(positions) != 6: reasons.append(f"robot {side} hand position is incomplete") elif len(states) != 6 or any( state not in (0, 1, 2, 3) for state in states ): # BrainCo MotorState: 0=idle, 1=running, 2=stall/contact or # limit, 3=turbo/continuous force, 255=unknown. Running and # contact are normal operating states; the vendor driver # retains its own current, stall and limit protection. reasons.append( f"robot {side} hand state is unknown/invalid or incomplete" ) return reasons @staticmethod def _positions(data: dict[str, Any]) -> list[float]: position = data["arm"]["position"] return [float(v) for v in [*position["left"], *position["right"]]] def _hand_targets(self, data: dict[str, Any]) -> dict[str, list[int]]: raw_positions = data["hand"]["position"] open_pose = [float(value) for value in self.hands_cfg["open_normalized"]] closed_pose = [float(value) for value in self.hands_cfg["closed_normalized"]] if len(open_pose) != 6 or len(closed_pose) != 6: raise ValueError("hand open/closed poses must each contain 6 values") if not all( math.isfinite(value) and 0.0 <= value <= 1.0 for value in [*open_pose, *closed_pose] ): raise ValueError("hand open/closed poses must be finite values in [0, 1]") minimum = int(self.hands_cfg["position_min"]) maximum = int(self.hands_cfg["position_max"]) if minimum < 0 or maximum <= minimum: raise ValueError("invalid BrainCo position range") targets: dict[str, list[int]] = {} for side in HAND_SIDES: raw = raw_positions[side] if isinstance(raw, (int, float)) and not isinstance(raw, bool): scalar = float(raw) if not math.isfinite(scalar) or scalar < -0.05 or scalar > 1.05: raise ValueError(f"{side} scalar must be in [0, 1]") scalar = max(0.0, min(1.0, scalar)) if bool(self.hands_cfg.get("invert_scalar", False)): scalar = 1.0 - scalar normalized = [ opened + scalar * (closed - opened) for opened, closed in zip(open_pose, closed_pose) ] elif isinstance(raw, list) and len(raw) == 6: normalized = [float(value) for value in raw] if not all( math.isfinite(value) and -0.05 <= value <= 1.05 for value in normalized ): raise ValueError(f"{side} 6-D target must be in [0, 1]") normalized = [max(0.0, min(1.0, value)) for value in normalized] else: raise ValueError(f"{side} target must be a scalar or 6-D list") targets[side] = [ int(round(minimum + value * (maximum - minimum))) for value in normalized ] return targets @staticmethod def _limit_reasons(positions: list[float], cfg: dict[str, Any]) -> list[str]: lower = [float(v) for v in cfg["joint_lower_rad"]] upper = [float(v) for v in cfg["joint_upper_rad"]] margin = float(cfg["joint_limit_margin_rad"]) if len(lower) != 14 or len(upper) != 14: return ["configuration must define 14 joint limits"] bad = [ JOINT_NAMES[i] for i, value in enumerate(positions) if value < lower[i] + margin or value > upper[i] - margin ] return ["joint target outside safe limit: " + ",".join(bad)] if bad else [] def _home_config_reasons(self) -> list[str]: home_cfg = self.cfg["home"] try: goal = [float(v) for v in home_cfg["joint_goal_rad"]] values = [ float(home_cfg["slew_rad_s"]), float(home_cfg["max_tracking_error_rad"]), float(home_cfg["command_tolerance_rad"]), float(home_cfg["actual_tolerance_rad"]), float(home_cfg["settle_s"]), float(home_cfg["timeout_s"]), ] except (KeyError, TypeError, ValueError): return ["invalid return-home configuration"] if len(goal) != 14 or not all(math.isfinite(v) for v in [*goal, *values]): return ["return-home configuration must contain finite values for 14 joints"] if any(v <= 0.0 for v in values): return ["return-home speed, tolerances and timeouts must be positive"] return self._limit_reasons(goal, self.cfg["control"]) def _slew_limit(self, desired: list[float], now: float) -> list[float]: if self.last_command is None: return list(desired) dt = max(0.001, now - self.last_publish_at) max_step = float(self.cfg["control"]["max_slew_rad_s"]) * dt return [ previous + max(-max_step, min(max_step, target - previous)) for previous, target in zip(self.last_command, desired) ] def _slew_hand(self, side: str, desired: list[int], now: float) -> list[int]: previous = self.last_hand_commands[side] if previous is None or len(previous) != 6: return list(desired) dt = max(0.001, now - self.last_hand_publish_at) max_step = float(self.hands_cfg["slew_units_per_s"]) * dt return [ int(round(current + max(-max_step, min(max_step, target - current)))) for current, target in zip(previous, desired) ] def _write_status( self, now: float, sample: ArmSnapshot | None, reasons: list[str] ) -> None: combo_elapsed = 0.0 if self.combo_started_at is not None: combo_elapsed = now - self.combo_started_at source_metrics = self.source.metrics() status = { "mode": "active-capable" if self.allow_publish else "monitor-only", "armed": self.armed, "returning_home": self.returning_home, "home_status": self.home_status, "home_speed_rad_s": self.cfg["home"]["slew_rad_s"], "home_goal_rad": self.cfg["home"]["joint_goal_rad"], "auto_home_on_stop": bool( self.cfg["control"].get("auto_home_on_stop", True) ), "custom_joint_limit_policy": "startup_only", "custom_hand_feedback_policy": ( "startup_qualifies_teleop; runtime_gap_pauses_hands_only" ), "data_collection_enabled": self.data_collection_enabled, "data_collection_binding": ( "left joystick press (L3), hold " f"{self.data_collection_gate.hold_seconds:.1f}s toggle; " f"release {self.data_collection_gate.release_seconds:.1f}s" ), "data_collection_gate_state": self.data_collection_gate.state, "data_collection_requested_active": self.data_collection_gate.active, "data_collection_requires_release": ( self.data_collection_gate.require_release ), "data_collection_button_pressed": ( self.data_collection_gate.button_pressed ), "data_collection_hold_s": ( 0.0 if self.data_collection_gate.hold_started_at is None else round(now - self.data_collection_gate.hold_started_at, 2) ), "data_collection_capture_id": self.data_capture_id, "data_collection_session_id": self.data_capture_session_id, "data_collection_toggle_count": self.data_toggle_count, "data_collection_last_transition": self.data_last_transition, "data_collection_pending_command": ( None if self.data_pending_control is None else self.data_pending_control.get("command") ), "data_collection_pending_age_s": ( None if self.data_pending_control is None or self.data_pending_control_since == 0.0 else round(now - self.data_pending_control_since, 3) ), "data_recorder_status": self.data_recorder_status, "data_recorder_status_age_s": ( None if self.data_recorder_status_at == 0.0 else round(now - self.data_recorder_status_at, 3) ), "runtime_hand_output_ready": self.hand_output_ready, "runtime_hand_output_reasons": self.runtime_hand_output_reasons, "safety_ready": not reasons, "safety_reasons": reasons, "iarm_transport": self.source.transport, "iarm_endpoint": self.source.description, "iarm_transport_status": source_metrics, "iarm_watchdog_restart_after_s": self.cfg["network"].get( "omnisocket_restart_after_stale_s" ), "iarm_connected": sample is not None, "iarm_age_s": None if sample is None else round(now - sample.received_at, 4), "iarm_id": None if sample is None else sample.data.get("isomorphic_arm_id"), "iarm_frequency_hz": None if sample is None else sample.data.get("freq"), "rl_state": self.rl_state, "rl_state_age_s": None if self.rl_state_at == 0.0 else round(now - self.rl_state_at, 4), "robot_arm_state_age_s": None if self.robot_arm_at == 0.0 else round(now - self.robot_arm_at, 4), "robot_arm_position_rad": self.robot_arm_positions, "robot_arm_errors": self.robot_arm_errors, "foreign_source_seen": self.foreign_source_seen, "operator_session_state": self.last_session_state, "operator_session_id": self.active_session_id, "operator_session_start_attempted_id": self.session_start_attempted_id, "operator_session_stop_reason": self.last_session_stop_reason, "brainco_hands_enabled": self.hands_enabled, "iarm_hand_position": None if sample is None else sample.data.get("hand", {}).get("position"), "robot_hand_positions": self.robot_hand_positions, "robot_hand_states": self.robot_hand_states, "robot_hand_state_age_s": { side: None if self.robot_hand_at[side] == 0.0 else round(now - self.robot_hand_at[side], 4) for side in HAND_SIDES }, "last_hand_commands": self.last_hand_commands, "last_hand_publish_age_s": None if self.last_hand_publish_at == 0.0 else round(now - self.last_hand_publish_at, 4), "hand_publish_count": self.hand_publish_count, "foreign_hand_source_seen": self.foreign_hand_source_seen, "right_point_gesture_enabled": self.right_point_gesture_enabled, "right_point_gesture_binding": ( "right_B hold " f"{self.right_point_gesture.hold_seconds:.1f}s bilateral toggle" ), "right_point_gesture_active": ( self.right_point_gesture.active if self.right_point_gesture_enabled else False ), "right_point_gesture_state": ( self.right_point_gesture.state if self.right_point_gesture_enabled else "disabled" ), "right_point_gesture_hold_s": round( self.right_point_gesture.hold_elapsed(now), 2 ), "right_point_gesture_release_s": round( self.right_point_gesture.release_elapsed(now), 2 ), "right_point_gesture_requires_release": ( self.right_point_gesture.require_release if self.right_point_gesture_enabled else False ), "right_point_gesture_freezing_input": ( self.right_point_gesture.freeze_right_hand if self.right_point_gesture_enabled else False ), "right_point_gesture_toggle_count": ( self.right_point_gesture.toggle_count if self.right_point_gesture_enabled else 0 ), "right_point_gesture_last_transition": ( self.right_point_gesture.last_transition if self.right_point_gesture_enabled else "disabled" ), "right_point_gesture_target_normalized": ( self.right_point_gesture_pose if self.right_point_gesture_enabled else None ), "right_point_gesture_target_positions": ( self.right_point_gesture_target if self.right_point_gesture_enabled else None ), "right_a_pose_enabled": self.right_a_pose_enabled, "right_a_pose_binding": "right_A hold; release restores live input", "right_a_pose_active": ( self.right_a_pose.active if self.right_a_pose_enabled else False ), "right_a_pose_state": ( self.right_a_pose.state if self.right_a_pose_enabled else "disabled" ), "right_a_pose_requires_release": ( self.right_a_pose.require_release if self.right_a_pose_enabled else False ), "right_a_pose_release_s": ( round(self.right_a_pose.release_elapsed(now), 2) if self.right_a_pose_enabled else 0.0 ), "right_a_pose_activation_count": ( self.right_a_pose.activation_count if self.right_a_pose_enabled else 0 ), "right_a_pose_last_transition": ( self.right_a_pose.last_transition if self.right_a_pose_enabled else "disabled" ), "right_a_pose_target_positions": ( self.right_a_pose_target if self.right_a_pose_enabled else None ), "locomotion_enabled": self.locomotion_enabled, "locomotion_binding": ( "left_stick_vertical; right_stick_horizontal, direct" ), "locomotion_active": self.walk_active, "locomotion_requires_neutral": self.walk_require_neutral, "locomotion_hold_s": 0.0, "locomotion_command": { "linear_x_m_s": self.walk_command[0], "angular_z_rad_s": self.walk_command[1], }, "locomotion_limits": { "forward_m_s": self.locomotion_cfg.get("max_forward_m_s"), "reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"), "angular_rad_s": self.locomotion_cfg.get("max_angular_rad_s"), "forward_expo": self.locomotion_cfg.get("joystick_expo"), "yaw_expo": self.locomotion_cfg.get( "yaw_joystick_expo", self.locomotion_cfg.get("joystick_expo"), ), }, "locomotion_publish_count": self.walk_publish_count, "locomotion_fsm_publish_enabled": False, "last_locomotion_publish_age_s": None if self.last_walk_publish_at == 0.0 else round(now - self.last_walk_publish_at, 4), "head_pitch_enabled": self.head_enabled, "head_pitch_binding": "left_Z + right_stick_vertical after neutral", "head_pitch_left_z_pressed": self.head_z_pressed, "head_pitch_active": self.head_pitch_controller.active, "head_pitch_state": self.head_pitch_controller.last_transition, "head_pitch_requires_neutral": ( self.head_pitch_controller.require_neutral ), "head_pitch_target_rad": self.head_pitch_controller.target_rad, "head_pitch_velocity_rad_s": ( self.head_pitch_controller.velocity_rad_s ), "head_pitch_feedback_rad": self.robot_head_positions.get( int(self.head_cfg.get("pitch_motor_id", 2)) ), "head_pitch_feedback_error": self.robot_head_errors.get( int(self.head_cfg.get("pitch_motor_id", 2)) ), "head_pitch_feedback_age_s": None if self.robot_head_at == 0.0 else round(now - self.robot_head_at, 4), "head_pitch_output_reasons": self.head_output_reasons, "head_pitch_limits_rad": { "up": self.head_pitch_controller.min_pitch_rad, "down": self.head_pitch_controller.max_pitch_rad, "max_speed": self.head_pitch_controller.max_speed_rad_s, "max_accel": self.head_pitch_controller.max_accel_rad_s2, }, "head_pitch_foreign_command_age_s": None if self.foreign_head_command_at == 0.0 else round(now - self.foreign_head_command_at, 4), "head_pitch_foreign_command_count": self.foreign_head_command_count, "head_pitch_publish_count": self.head_publish_count, "last_head_pitch_publish_age_s": None if self.last_head_publish_at == 0.0 else round(now - self.last_head_publish_at, 4), "unsupported_binding": ( "right_C + right_A (C+A) is not registered by xTELE 0.1.2" ), "start_stop_combo": ( "EAI-gated left_Z + right_C, hold 3s; matching STOP triggers Home" if self.source.transport == "omnisocket" else "local left_Z + right_C, hold 3s; stop triggers Home" ), "combo_hold_s": round(combo_elapsed, 2), "last_publish_age_s": None if self.last_publish_at == 0.0 else round(now - self.last_publish_at, 4), "updated_unix_s": time.time(), } try: self.status_file.parent.mkdir(parents=True, exist_ok=True) temporary = self.status_file.with_suffix(self.status_file.suffix + ".tmp") temporary.write_text(json.dumps(status, ensure_ascii=False, indent=2) + "\n") os.replace(temporary, self.status_file) except OSError as exc: self.get_logger().error(f"cannot write status file: {exc}") def load_config(path: Path) -> dict[str, Any]: with path.open("rb") as handle: return tomllib.load(handle) def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--config", type=Path, required=True) parser.add_argument( "--allow-publish", action="store_true", help=( "allow a validated EAI START session (or direct-ZMQ fallback " "combo) to arm ROS publication" ), ) parser.add_argument( "--status-file", type=Path, default=Path("/tmp/tg3_local_teleop_status.json") ) parser.add_argument( "--duration", type=float, default=0.0, help="exit after this many seconds (used for monitor-only validation)", ) return parser.parse_args() def main() -> int: args = parse_args() config = load_config(args.config) rclpy.init() node = LocalTeleopBridge(config, args.allow_publish, args.status_file) deadline = time.monotonic() + args.duration if args.duration > 0 else None try: while rclpy.ok() and (deadline is None or time.monotonic() < deadline): rclpy.spin_once(node, timeout_sec=0.1) except KeyboardInterrupt: pass finally: node.close() node.destroy_node() rclpy.shutdown() return 0 if __name__ == "__main__": raise SystemExit(main())