Saqr/saqr_g1_bridge.py

#!/usr/bin/env python3
"""
saqr_g1_bridge.py

Bridge between Saqr PPE detection and the Unitree G1 robot.

Default behavior on the robot: do NOTHING until the operator presses
**R2+X** on the G1 wireless remote. R2+X starts saqr.py as a subprocess,
R2+Y stops it. While Saqr is running the bridge parses its event stream and:

  * UNSAFE  -> announce missing PPE via the G1 onboard TtsMaker (English,
              speaker_id=2) AND run the 'reject' arm action (id=13).
  * SAFE    -> announce "Safe to enter. Have a good day." No arm motion.
  * PARTIAL -> nothing.

Both DDS clients (G1ArmActionClient + G1 AudioClient) and the LowState
subscriber share a single ChannelFactoryInitialize call. The TTS speaker_id
was identified by running Project/Sanad/voice_example.py mode 6 — speaker_id=2
is English on current G1 firmware (speaker_id=0 is Chinese regardless of input
text).

Saqr event line format (from emit_event in saqr.py):
    ID 0001 | NEW | UNSAFE | wearing: ... | missing: ... | unknown: ...
    ID 0001 | STATUS_CHANGE | SAFE  | wearing: ... | missing: ... | unknown: ...

Usage:
    # on the robot — wait for R2+X / R2+Y to start/stop Saqr
    python3 saqr_g1_bridge.py --iface eth0 --source realsense --headless

    # legacy mode: start saqr immediately and ignore the controller
    python3 saqr_g1_bridge.py --no-trigger --source 0 --headless

    # dry run (no robot movement / TTS, just print decisions)
    python3 saqr_g1_bridge.py --dry-run --no-trigger --source 0 --headless

    # forward extra args to saqr.py after a `--`
    python3 saqr_g1_bridge.py --iface eth0 -- --conf 0.4 --imgsz 640
"""

from __future__ import annotations

import argparse
import collections
import datetime
import os
import re
import signal
import subprocess
import sys
import threading
import time
from pathlib import Path
from typing import Deque, Dict, Optional


def _ts() -> str:
    """HH:MM:SS.fff timestamp string for log lines."""
    return datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3]


# ── Defaults ─────────────────────────────────────────────────────────────────
HERE         = Path(__file__).resolve().parent
SAQR_DIR     = HERE                                  # bridge lives next to saqr.py
SAQR_SCRIPT  = SAQR_DIR / "saqr.py"

DANGER_STATUS = "UNSAFE"
SAFE_STATUS   = "SAFE"
REJECT_ACTION = "reject"
RELEASE_ACTION = "release arm"

# G1 onboard TtsMaker (see Project/Sanad/voice_example.py mode 6).
# speaker_id=2 was confirmed English on current G1 firmware.
TTS_SPEAKER_ID = 2
TTS_VOLUME     = 100

TTS_TEXT_SAFE            = "Safe to enter. Have a good day."
TTS_UNSAFE_WITH_MISSING  = (
    "Please stop. Wear your proper safety equipment. You are missing {items}."
)
TTS_UNSAFE_GENERIC       = (
    "Please stop. Wear your proper safety equipment."
)
TTS_BRIDGE_DEACTIVATED   = "Saqr deactivated."
TTS_BRIDGE_READY         = "Saqr is running. Press R2 plus X to start."
TTS_BRIDGE_NO_CAMERA     = (
    "Camera not connected. Please plug in the camera and try again."
)
# Note: there is no per-start "Saqr activated" announcement on purpose. It
# was colliding with the very first safety phrase out of saqr (the SDK
# returned 3104 = device busy and dropped the safety audio). R2+X gives
# the operator tactile feedback already.

# G1 TtsMaker is non-blocking and rejects overlapping phrases with code 3104
# (device busy). To avoid dropped speech we run TTS on a dedicated worker
# thread which **inline-sleeps** for the expected playback duration of each
# phrase, so the next phrase only fires after the previous one is done.
# speak() itself is non-blocking — it just enqueues — so the arm reject
# action runs in parallel with the spoken phrase.
TTS_SECONDS_PER_CHAR = 0.12    # empirical baseline on current G1 firmware
TTS_MIN_SECONDS      = 2.5     # floor: very short phrases still need a beat
TTS_QUEUE_MAX        = 4       # newest queued; oldest dropped on overflow
# Adaptive busy multiplier — grows on each 3104, shrinks slowly when calls
# succeed. Bounded so it can never freeze the worker forever.
TTS_BUSY_FACTOR_MIN  = 1.0
TTS_BUSY_FACTOR_MAX  = 2.5
TTS_BUSY_FACTOR_UP   = 1.20    # multiplied on each 3104
TTS_BUSY_FACTOR_DOWN = 0.97    # multiplied on each clean call

# If saqr exits with non-zero rc within this many seconds of start_saqr(),
# treat it as a failed launch (e.g. RealSense unplugged) and announce it.
QUICK_FAIL_WINDOW_S = 8.0

# ID NNNN | EVENT_TYPE | STATUS | wearing: ... | missing: ... | unknown: ...
EVENT_RE = re.compile(
    r"^ID\s+(?P<id>\d+)\s*\|\s*"
    r"(?P<event>NEW|STATUS_CHANGE)\s*\|\s*"
    r"(?P<status>SAFE|PARTIAL|UNSAFE)\s*\|\s*"
    r"wearing:\s*(?P<wearing>[^|]*?)\s*\|\s*"
    r"missing:\s*(?P<missing>[^|]*?)\s*\|\s*"
    r"unknown:\s*(?P<unknown>.*?)\s*$"
)


def _parse_list_field(s: str) -> list:
    """Parse 'helmet, vest' or 'none' into a list of items."""
    s = (s or "").strip()
    if not s or s.lower() == "none":
        return []
    return [x.strip() for x in s.split(",") if x.strip()]


def _human_join(items: list) -> str:
    """Join a list in natural English: 'helmet and vest', 'a, b, and c'."""
    if not items:
        return ""
    if len(items) == 1:
        return items[0]
    if len(items) == 2:
        return f"{items[0]} and {items[1]}"
    return ", ".join(items[:-1]) + f", and {items[-1]}"


def build_unsafe_tts(missing: list) -> str:
    if not missing:
        return TTS_UNSAFE_GENERIC
    return TTS_UNSAFE_WITH_MISSING.format(items=_human_join(missing))


# ── G1 robot controller (lazy import: SDK only loaded when not in dry-run) ───
class RobotController:
    """Owns both the G1 arm action client and the G1 audio (TTS) client.

    A single ChannelFactoryInitialize call is shared by both clients.
    """

    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

        # TTS pacing — see TtsMaker code 3104 ("device busy") notes.
        # speak() enqueues; a dedicated worker thread does the blocking calls.
        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 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 TTS — enqueue the phrase for the worker thread.

        Returns immediately so callers (e.g. the bridge's reject arm action)
        can run in parallel with the spoken phrase. The worker thread paces
        TtsMaker calls so the SDK doesn't reject overlapping phrases (3104).
        """
        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
        # Drop adjacent duplicates: if the same phrase is already at the
        # back of the queue, don't enqueue another copy.
        if self._tts_queue and self._tts_queue[-1] == text:
            return
        # deque(maxlen=N) drops the oldest entry on overflow, which is the
        # right policy for safety announcements: the newest event is most
        # relevant.
        self._tts_queue.append(text)
        self._tts_event.set()

    def shutdown_tts(self):
        """Stop the TTS worker thread (used during bridge shutdown)."""
        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):
        """Single TTS call. Blocks the WORKER thread for the expected playback
        duration so the next queued phrase only fires after this one is done.
        Caller threads (reader / trigger / main) are unaffected.
        """
        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()

        # Block the worker until the phrase is expected to finish playing.
        # Account for time already spent inside the SDK call.
        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])


# ── Bridge ───────────────────────────────────────────────────────────────────
class Bridge:
    """Owns the saqr.py subprocess lifecycle and the event-stream parser."""

    def __init__(
        self,
        robot: RobotController,
        cooldown_s: float,
        release_after_s: float,
        saqr_args: list,
        env: Dict[str, str],
    ):
        self.robot = robot
        self.cooldown_s = cooldown_s
        self.release_after_s = release_after_s
        self.saqr_args = saqr_args
        self.env = env

        # Event-state tracking — cleared on each saqr (re)start so a stale
        # SAFE doesn't suppress an UNSAFE on the next session.
        self.last_status: Dict[int, str] = {}
        # Per-(id, status) cooldown so SAFE and UNSAFE timers are independent.
        self.last_trigger_t: Dict[tuple, float] = {}
        self._state_lock = threading.Lock()

        # Subprocess state.
        self.proc: Optional[subprocess.Popen] = None
        self.reader_thread: Optional[threading.Thread] = None
        self._proc_lock = threading.Lock()
        self._proc_start_t: float = 0.0

    # ── Subprocess control ─────────────────────────────────────────────────
    def is_running(self) -> bool:
        with self._proc_lock:
            return self.proc is not None and self.proc.poll() is None

    def start_saqr(self):
        with self._proc_lock:
            if self.proc is not None and self.proc.poll() is None:
                print("[BRIDGE] start ignored — saqr already running", flush=True)
                return

            cmd = build_saqr_cmd(self.saqr_args)
            print(f"[BRIDGE] starting saqr: {' '.join(cmd)}", flush=True)
            self.proc = subprocess.Popen(
                cmd,
                cwd=str(SAQR_DIR),
                stdout=subprocess.PIPE,
                stderr=subprocess.STDOUT,
                bufsize=1,
                text=True,
                env=self.env,
            )
            self._proc_start_t = time.time()

            with self._state_lock:
                self.last_status.clear()
                self.last_trigger_t.clear()

            self.reader_thread = threading.Thread(
                target=self._read_stdout,
                args=(self.proc,),
                daemon=True,
            )
            self.reader_thread.start()
        # No "Saqr activated." TTS — it kept colliding with the first safety
        # phrase from saqr (the SDK reported 3104 and dropped the safety
        # audio). The R2+X press itself is enough operator feedback.

    def stop_saqr(self):
        with self._proc_lock:
            proc = self.proc
            if proc is None or proc.poll() is not None:
                print("[BRIDGE] stop ignored — saqr not running", flush=True)
                self.proc = None
                return
            print("[BRIDGE] stopping saqr (SIGINT)", flush=True)
            try:
                proc.send_signal(signal.SIGINT)
            except Exception:
                pass

        # Wait outside the proc lock so the reader thread can drain stdout.
        try:
            proc.wait(timeout=3.0)
        except subprocess.TimeoutExpired:
            print("[BRIDGE] saqr did not exit in 3s, sending SIGTERM", flush=True)
            try:
                proc.terminate()
                proc.wait(timeout=2.0)
            except subprocess.TimeoutExpired:
                print("[BRIDGE] saqr unresponsive, sending SIGKILL", flush=True)
                proc.kill()
                proc.wait()

        if self.reader_thread is not None:
            self.reader_thread.join(timeout=2.0)

        with self._proc_lock:
            self.proc = None
            self.reader_thread = None

        self.robot.speak(TTS_BRIDGE_DEACTIVATED)

    def _read_stdout(self, proc: subprocess.Popen):
        start_t = self._proc_start_t
        try:
            assert proc.stdout is not None
            for line in proc.stdout:
                self.handle_line(line)
        except Exception as e:
            print(f"[BRIDGE][ERR] reader thread: {e}", flush=True)
        rc = proc.wait()
        lifetime = time.time() - start_t if start_t > 0 else 0.0
        print(f"[BRIDGE] saqr exited rc={rc} (lifetime={lifetime:.1f}s)",
              flush=True)

        # If saqr died quickly with a non-zero rc, it almost always means the
        # camera (RealSense / V4L2) couldn't be opened. Tell the operator out
        # loud and stay idle waiting for the next R2+X.
        if rc not in (0, -2) and 0 < lifetime < QUICK_FAIL_WINDOW_S:
            try:
                self.robot.speak(TTS_BRIDGE_NO_CAMERA)
            except Exception as e:
                print(f"[BRIDGE][ERR] no-camera tts failed: {e}", flush=True)

    # ── Event parsing ──────────────────────────────────────────────────────
    def handle_line(self, line: str):
        line = line.rstrip()
        if not line:
            return
        # Always echo Saqr output so the user still sees the live stream.
        print(line, flush=True)

        m = EVENT_RE.match(line)
        if not m:
            return

        track_id = int(m.group("id"))
        status = m.group("status")
        missing = _parse_list_field(m.group("missing"))

        with self._state_lock:
            prev = self.last_status.get(track_id)
            self.last_status[track_id] = status

            # Only SAFE / UNSAFE transitions trigger the robot. PARTIAL is silent.
            if status not in (DANGER_STATUS, SAFE_STATUS):
                return

            # Only fire on transitions, not on every NEW/STATUS_CHANGE for the
            # same status.
            if prev == status:
                return

            now = time.time()
            last_t = self.last_trigger_t.get((track_id, status), 0.0)
            if (now - last_t) < self.cooldown_s:
                return
            self.last_trigger_t[(track_id, status)] = now

        # Run robot actions outside the lock so we don't block parsing.
        try:
            if status == DANGER_STATUS:
                self.robot.speak(build_unsafe_tts(missing))
                self.robot.reject(release_after=self.release_after_s)
            else:  # SAFE
                self.robot.speak(TTS_TEXT_SAFE)
        except Exception as e:
            print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True)


# ── Trigger polling loop ─────────────────────────────────────────────────────
def trigger_loop(bridge: Bridge, hub, stop_event: threading.Event,
                 poll_hz: float = 50.0):
    """Watch the wireless remote for R2+X (start) and R2+Y (stop).

    Both combos are rising-edge triggered with a release-wait debounce so a
    held button only fires once.
    """
    period = 1.0 / max(poll_hz, 1.0)
    waiting_release_x = False
    waiting_release_y = False
    print("[BRIDGE] trigger loop ready — press R2+X to start, R2+Y to stop.",
          flush=True)
    while not stop_event.is_set():
        time.sleep(period)
        if not hub.first_state:
            continue

        r2x = hub.combo_r2x()
        r2y = hub.combo_r2y()

        # R2+X — start
        if waiting_release_x:
            if not r2x:
                waiting_release_x = False
        elif r2x:
            waiting_release_x = True
            print("[BRIDGE] R2+X pressed -> start saqr", flush=True)
            try:
                bridge.start_saqr()
            except Exception as e:
                print(f"[BRIDGE][ERR] start_saqr failed: {e}", flush=True)

        # R2+Y — stop
        if waiting_release_y:
            if not r2y:
                waiting_release_y = False
        elif r2y:
            waiting_release_y = True
            print("[BRIDGE] R2+Y pressed -> stop saqr", flush=True)
            try:
                bridge.stop_saqr()
            except Exception as e:
                print(f"[BRIDGE][ERR] stop_saqr failed: {e}", flush=True)


# ── Saqr subprocess command builder ──────────────────────────────────────────
def build_saqr_cmd(saqr_extra_args: list) -> list:
    if not SAQR_SCRIPT.exists():
        sys.exit(f"[BRIDGE][FATAL] saqr.py not found at: {SAQR_SCRIPT}")
    # -u for unbuffered stdout (so events arrive line-by-line).
    return [sys.executable, "-u", str(SAQR_SCRIPT), *saqr_extra_args]


def split_argv(argv: list[str]) -> tuple[list[str], list[str]]:
    """Split bridge args from saqr passthrough args at the first '--'."""
    if "--" in argv:
        idx = argv.index("--")
        return argv[:idx], argv[idx + 1 :]
    return argv, []


def main():
    bridge_argv, saqr_extra = split_argv(sys.argv[1:])

    ap = argparse.ArgumentParser(
        description="Bridge Saqr PPE events to the G1 arm 'reject' action."
    )
    ap.add_argument("--iface", default=None,
                    help="DDS network interface (e.g. enp3s0). Optional.")
    ap.add_argument("--timeout", type=float, default=10.0,
                    help="G1 arm client timeout (seconds).")
    ap.add_argument("--cooldown", type=float, default=8.0,
                    help="Per-track-id seconds before reject can re-trigger.")
    ap.add_argument("--release-after", type=float, default=2.0,
                    help="Seconds before auto-running 'release arm' (0 = never).")
    ap.add_argument("--dry-run", action="store_true",
                    help="Parse and decide but never call the SDK.")
    ap.add_argument("--speaker-id", type=int, default=TTS_SPEAKER_ID,
                    help=f"G1 TtsMaker speaker_id (default {TTS_SPEAKER_ID}, English).")
    ap.add_argument("--no-trigger", action="store_true",
                    help="Skip the wireless-remote trigger loop and start saqr "
                         "immediately (legacy / dev mode).")

    # Convenience pass-throughs to saqr.py (you can also use `-- ...`).
    ap.add_argument("--source", default=None,
                    help="Saqr --source (0/realsense/path). Default: leave to saqr.")
    ap.add_argument("--headless", action="store_true",
                    help="Pass --headless to saqr.")
    ap.add_argument("--saqr-conf", type=float, default=None,
                    help="Pass --conf to saqr.")
    ap.add_argument("--imgsz", type=int, default=None,
                    help="Pass --imgsz to saqr.")
    ap.add_argument("--device", default=None,
                    help="Pass --device to saqr (e.g. cpu / 0 / cuda:0).")

    args = ap.parse_args(bridge_argv)

    # Build saqr args from convenience flags + raw passthrough.
    saqr_args: list[str] = []
    if args.source is not None:
        saqr_args += ["--source", args.source]
    if args.headless:
        saqr_args += ["--headless"]
    if args.saqr_conf is not None:
        saqr_args += ["--conf", str(args.saqr_conf)]
    if args.imgsz is not None:
        saqr_args += ["--imgsz", str(args.imgsz)]
    if args.device is not None:
        saqr_args += ["--device", args.device]
    saqr_args += saqr_extra

    use_trigger = not args.no_trigger and not args.dry_run

    robot = RobotController(
        iface=args.iface,
        timeout=args.timeout,
        dry_run=args.dry_run,
        tts_speaker_id=args.speaker_id,
        want_lowstate=use_trigger,
    )

    env = os.environ.copy()
    env["PYTHONUNBUFFERED"] = "1"

    bridge = Bridge(
        robot=robot,
        cooldown_s=args.cooldown,
        release_after_s=args.release_after,
        saqr_args=saqr_args,
        env=env,
    )
    print(f"[BRIDGE] saqr cmd template: {' '.join(build_saqr_cmd(saqr_args))}",
          flush=True)
    print(f"[BRIDGE] cwd: {SAQR_DIR}", flush=True)

    stop_event = threading.Event()

    def _forward_signal(signum, _frame):
        print(f"[BRIDGE] signal {signum} -> shutting down", flush=True)
        stop_event.set()

    signal.signal(signal.SIGINT, _forward_signal)
    signal.signal(signal.SIGTERM, _forward_signal)

    # Decide which mode to run in.
    have_hub = use_trigger and robot.hub is not None
    if use_trigger and not have_hub:
        print("[BRIDGE][WARN] --no-trigger not set, but no LowState hub is "
              "available. Falling back to legacy auto-start mode.", flush=True)

    trigger_thread: Optional[threading.Thread] = None
    try:
        if have_hub:
            # Wireless-remote mode: idle until R2+X.
            # Announce readiness so the operator knows the bridge is alive
            # before they reach for the wireless remote.
            try:
                robot.speak(TTS_BRIDGE_READY)
            except Exception as e:
                print(f"[BRIDGE][WARN] startup announce failed: {e}", flush=True)

            trigger_thread = threading.Thread(
                target=trigger_loop,
                args=(bridge, robot.hub, stop_event),
                daemon=True,
            )
            trigger_thread.start()

            # Park the main thread until SIGINT/SIGTERM.
            while not stop_event.is_set():
                time.sleep(0.2)
        else:
            # Legacy mode: start saqr immediately and run until it (or we) exits.
            bridge.start_saqr()
            while not stop_event.is_set() and bridge.is_running():
                time.sleep(0.2)
    finally:
        # Make sure saqr is stopped before we exit, regardless of mode.
        if bridge.is_running():
            bridge.stop_saqr()
        stop_event.set()
        if trigger_thread is not None:
            trigger_thread.join(timeout=1.0)
        try:
            robot.shutdown_tts()
        except Exception:
            pass
        print("[BRIDGE] bye.", flush=True)
        sys.exit(0)


if __name__ == "__main__":
    main()