217 lines
8.0 KiB
Python
217 lines
8.0 KiB
Python
"""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])
|