"""G1 arm + audio + LowState DDS client owned by the bridge. A dedicated TTS worker thread paces ``TtsMaker`` calls so overlapping phrases don't trip the SDK's "device busy" error (3104). The busy multiplier adapts up on 3104s and decays on clean calls. """ from __future__ import annotations import collections import datetime import threading import time from typing import Deque, Optional TTS_VOLUME = 100 TTS_SECONDS_PER_CHAR = 0.12 TTS_MIN_SECONDS = 2.5 TTS_QUEUE_MAX = 4 TTS_BUSY_FACTOR_MIN = 1.0 TTS_BUSY_FACTOR_MAX = 2.5 TTS_BUSY_FACTOR_UP = 1.20 TTS_BUSY_FACTOR_DOWN = 0.97 REJECT_ACTION = "reject" RELEASE_ACTION = "release arm" def _ts() -> str: return datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3] class RobotController: """Owns both the G1 arm action client and the G1 audio (TTS) client. A single ``ChannelFactoryInitialize`` call is shared by both clients and the optional ``rt/lowstate`` subscriber used by the wireless-remote loop. """ def __init__(self, iface: Optional[str], timeout: float, dry_run: bool, tts_speaker_id: int, want_lowstate: bool = True): self.dry_run = dry_run self.tts_speaker_id = tts_speaker_id self.arm_client = None self.audio_client = None self._action_map = None self.hub = None self._lowstate_sub = None self._tts_queue: Deque[str] = collections.deque(maxlen=TTS_QUEUE_MAX) self._tts_event = threading.Event() self._tts_worker_stop = threading.Event() self._tts_worker_thread: Optional[threading.Thread] = None self._tts_busy_factor: float = TTS_BUSY_FACTOR_MIN self._tts_last_call_t: float = 0.0 self._tts_call_count: int = 0 self._tts_busy_count: int = 0 if dry_run: print("[BRIDGE] DRY RUN — G1 SDK will not be loaded.", flush=True) return from unitree_sdk2py.core.channel import ChannelFactoryInitialize from unitree_sdk2py.g1.arm.g1_arm_action_client import ( G1ArmActionClient, action_map, ) from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient self._action_map = action_map if iface: ChannelFactoryInitialize(0, iface) else: ChannelFactoryInitialize(0) self.arm_client = G1ArmActionClient() self.arm_client.SetTimeout(timeout) self.arm_client.Init() print(f"[BRIDGE] G1ArmActionClient ready (iface={iface or 'default'})", flush=True) self.audio_client = AudioClient() self.audio_client.SetTimeout(timeout) self.audio_client.Init() try: self.audio_client.SetVolume(TTS_VOLUME) except Exception as e: print(f"[BRIDGE][WARN] AudioClient.SetVolume failed: {e}", flush=True) print(f"[BRIDGE] G1 AudioClient ready (speaker_id={tts_speaker_id})", flush=True) self._tts_worker_thread = threading.Thread( target=self._tts_worker_loop, name="TtsWorker", daemon=True, ) self._tts_worker_thread.start() if want_lowstate: try: from unitree_sdk2py.core.channel import ChannelSubscriber from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ from saqr.robot.controller import LowStateHub self.hub = LowStateHub(watchdog_timeout=0.25) self._lowstate_sub = ChannelSubscriber("rt/lowstate", LowState_) self._lowstate_sub.Init(self.hub.handler, 10) print("[BRIDGE] Subscribed to rt/lowstate (wireless remote)", flush=True) except Exception as e: print(f"[BRIDGE][WARN] LowState subscribe failed: {e}", flush=True) print("[BRIDGE][WARN] Trigger keys (R2+X / R2+Y) will not work.", flush=True) self.hub = None # ── TTS ───────────────────────────────────────────────────────────────── def _estimate_tts_seconds(self, text: str) -> float: base = max(TTS_MIN_SECONDS, len(text) * TTS_SECONDS_PER_CHAR) return base * self._tts_busy_factor def speak(self, text: str): """Non-blocking — enqueue the phrase for the worker thread.""" if self.dry_run: print(f"[BRIDGE] (dry) would TtsMaker({text!r}, " f"speaker_id={self.tts_speaker_id})", flush=True) return if self.audio_client is None: return if self._tts_queue and self._tts_queue[-1] == text: return self._tts_queue.append(text) self._tts_event.set() def shutdown_tts(self): self._tts_worker_stop.set() self._tts_event.set() if self._tts_worker_thread is not None: self._tts_worker_thread.join(timeout=1.0) def _tts_worker_loop(self): while not self._tts_worker_stop.is_set(): if not self._tts_queue: self._tts_event.wait(timeout=0.2) self._tts_event.clear() continue try: text = self._tts_queue.popleft() except IndexError: continue self._speak_blocking(text) def _speak_blocking(self, text: str): if self.audio_client is None: return now = time.monotonic() gap_since_last = (now - self._tts_last_call_t) if self._tts_last_call_t else -1.0 est = self._estimate_tts_seconds(text) qsize = len(self._tts_queue) self._tts_call_count += 1 gap_str = f"{gap_since_last:5.2f}s" if gap_since_last >= 0 else " n/a" print( f"[BRIDGE {_ts()}] tts -> {text!r} " f"(est={est:.2f}s, gap={gap_str}, busy_x={self._tts_busy_factor:.2f}, " f"q={qsize})", flush=True, ) call_t0 = time.monotonic() try: code = self.audio_client.TtsMaker(text, self.tts_speaker_id) except Exception as e: print(f"[BRIDGE {_ts()}][ERR] TtsMaker raised: {e}", flush=True) return call_dt = time.monotonic() - call_t0 if code != 0: self._tts_busy_count += 1 self._tts_busy_factor = min( TTS_BUSY_FACTOR_MAX, self._tts_busy_factor * TTS_BUSY_FACTOR_UP ) print( f"[BRIDGE {_ts()}][WARN] TtsMaker rc={code} " f"(call took {call_dt*1000:.0f}ms; busy_x -> " f"{self._tts_busy_factor:.2f})", flush=True, ) else: self._tts_busy_factor = max( TTS_BUSY_FACTOR_MIN, self._tts_busy_factor * TTS_BUSY_FACTOR_DOWN ) self._tts_last_call_t = time.monotonic() remaining = est - call_dt if remaining > 0: time.sleep(remaining) # ── Arm ───────────────────────────────────────────────────────────────── def reject(self, release_after: float): if self.dry_run: print(f"[BRIDGE] (dry) would run '{REJECT_ACTION}' " f"then release after {release_after:.1f}s", flush=True) return if self.arm_client is None or self._action_map is None: return if REJECT_ACTION not in self._action_map: print(f"[BRIDGE][ERR] '{REJECT_ACTION}' not in SDK action_map", flush=True) return print(f"[BRIDGE] -> {REJECT_ACTION}", flush=True) self.arm_client.ExecuteAction(self._action_map[REJECT_ACTION]) if release_after > 0: time.sleep(release_after) print(f"[BRIDGE] -> {RELEASE_ACTION}", flush=True) self.arm_client.ExecuteAction(self._action_map[RELEASE_ACTION])