Update 2026-04-15 11:14:11

DEPLOY.md

@@ -48,7 +48,7 @@ cd ~/Robotics_workspace/AI/Saqr
 ssh unitree@192.168.123.164 "mkdir -p ~/Saqr/{models,captures/{SAFE,PARTIAL,UNSAFE},Config,Logs}"
 
 # Copy project files
-scp saqr.py saqr_g1_bridge.py detect.py manager.py logger.py gui.py requirements.txt deploy.sh DEPLOY.md \
+scp saqr.py saqr_g1_bridge.py controller.py detect.py manager.py logger.py gui.py requirements.txt deploy.sh DEPLOY.md \
     unitree@192.168.123.164:~/Saqr/
 
 # Copy config
@@ -205,55 +205,114 @@ python detect.py --source realsense --model models/saqr_best.pt
 
 ## Step 4b: Run with G1 TTS + Reject Action (Bridge)
 
-`saqr_g1_bridge.py` spawns `saqr.py`, parses its event stream, and drives the
+`saqr_g1_bridge.py` is the production entry point. It does **not** run Saqr
-G1 **onboard TTS** and the G1 **arm action client** on each per-person status
+itself — it sits idle, watches the G1 wireless remote, spawns `saqr.py` as a
-transition:
+subprocess on demand, and drives the G1 onboard TTS + arm action client from
+Saqr's event stream.
+
+### Wireless-remote workflow
+
+| Press | Action |
+|-------|--------|
+| **R2 + X** | Start `saqr.py`, robot says **"Saqr activated."** |
+| **R2 + Y** | Stop `saqr.py` (SIGINT → SIGTERM → SIGKILL escalation), robot says **"Saqr deactivated."** Bridge stays running, ready for the next R2+X. |
+| **Ctrl+C** in the terminal | Stop saqr (if running) and exit the bridge cleanly. |
+
+Each press is rising-edge debounced (release-wait), so holding the button
+only fires once. Track IDs and per-id status state are reset on every
+start, so a leftover SAFE from one session never suppresses an UNSAFE in
+the next.
+
+### Per-detection behavior (while saqr is running)
 
 | Transition | TTS (speaker_id=2, English) | Arm action |
 |------------|------------------------------|------------|
-| → UNSAFE   | "Not safe! Please wear your protective equipment." | `reject` (id=13) + auto `release arm` |
+| → UNSAFE   | "Please stop. Wear your proper safety equipment. You are missing **{items}**." (or generic text if no items reported) | `reject` (id=13) + auto `release arm` |
-| → SAFE     | "Safe." | — |
+| → SAFE     | "Safe to enter. Have a good day." | — |
 | → PARTIAL  | — | — |
 
-Requires `unitree_sdk2py` installed on the robot and a reachable DDS bus on
+The missing-item list is parsed live from Saqr's event line and joined in
-`eth0`. The bridge uses a single `ChannelFactoryInitialize` for both clients.
+natural English: `"vest"`, `"helmet and vest"`, `"helmet, vest, and gloves"`.
+
+### Architecture
+
+- One `ChannelFactoryInitialize(0, eth0)` is shared by **all** DDS clients:
+  - `G1ArmActionClient` — runs the `reject` arm action.
+  - `G1 AudioClient` — `TtsMaker(text, speaker_id)` for English speech.
+  - `ChannelSubscriber("rt/lowstate", LowState_)` — receives the wireless
+    remote button bits.
+- `controller.py` exposes `LowStateHub` + `UnitreeRemote`, parses the
+  `wireless_remote` byte field, and provides `combo_r2x()` / `combo_r2y()`.
+- A 50 Hz daemon thread polls the hub for rising edges and calls
+  `Bridge.start_saqr()` / `Bridge.stop_saqr()`.
+
+Requires `unitree_sdk2py` installed on the robot and a reachable DDS bus on
+`eth0`. Deploy `controller.py` alongside `saqr_g1_bridge.py` — without it the
+trigger loop is skipped and the bridge falls back to legacy auto-start mode.
+
+### Recommended: production run with R2+X / R2+Y
 
-### Headless + MJPEG stream (recommended over SSH):
 ```bash
 conda activate marcus      # or teleimager — whichever env has unitree_sdk2py
 cd ~/Saqr
 python3 saqr_g1_bridge.py --iface eth0 --source realsense --headless -- --stream 8080
 ```
-Then open `http://192.168.123.164:8080` in your laptop browser.
+Boot output should include:
+```
+[BRIDGE] G1ArmActionClient ready (iface=eth0)
+[BRIDGE] G1 AudioClient ready (speaker_id=2)
+[BRIDGE] Subscribed to rt/lowstate (wireless remote)
+[BRIDGE] trigger loop ready — press R2+X to start, R2+Y to stop.
+```
+Then press **R2+X** to begin, **R2+Y** to stop. The MJPEG stream is at
+`http://192.168.123.164:8080` (only while saqr is running).
+
+If `pyrealsense2` reports `No device connected`, fall back to the V4L2 path:
+```bash
+python3 saqr_g1_bridge.py --iface eth0 --source /dev/video2 --headless -- --stream 8080
+```
+
+### Live OpenCV window on the robot's physical monitor
 
-### With live OpenCV window (physical monitor on robot):
 ```bash
 xhost +local: >/dev/null 2>&1
 DISPLAY=:0 python3 saqr_g1_bridge.py --iface eth0 --source realsense
 ```
-`q` in the window quits; Ctrl+C in the terminal is also forwarded to Saqr.
+`q` in the OpenCV window quits the current saqr session (same as R2+Y).
+
+### Legacy / dev mode (no controller, no trigger)
+
+`--no-trigger` skips the wireless-remote subscription entirely and starts
+saqr immediately. Use this on the workstation or when you want the old
+"always running" behavior.
 
-### Dry run (no TTS, no motion — just see decisions):
 ```bash
-python3 saqr_g1_bridge.py --dry-run --source realsense --headless
+# On the workstation, no robot, no SDK:
+python3 saqr_g1_bridge.py --no-trigger --dry-run --source 0 --headless
+
+# On the robot, but skipping the trigger:
+python3 saqr_g1_bridge.py --no-trigger --iface eth0 --source realsense --headless
 ```
 
-### Bridge CLI flags:
+`--dry-run` automatically implies `--no-trigger` (no SDK = no LowState).
+
+### Bridge CLI flags
 
 | Flag | Default | Description |
 |------|---------|-------------|
 | `--iface` | *(default DDS)* | DDS network interface, e.g. `eth0` |
-| `--timeout` | `10.0` | Arm/Audio client timeout (seconds) |
+| `--timeout` | `10.0` | Arm/Audio/LowState client timeout (seconds) |
-| `--cooldown` | `8.0` | Per-(id, status) seconds before re-triggering |
+| `--cooldown` | `8.0` | Per-(track_id, status) seconds before re-triggering TTS/arm |
 | `--release-after` | `2.0` | Seconds before auto `release arm` (0 = never) |
 | `--speaker-id` | `2` | G1 `TtsMaker` speaker_id (2 = English on current firmware) |
-| `--dry-run` | off | Parse events but never call the SDK |
+| `--dry-run` | off | Parse events but never call the SDK; implies `--no-trigger` |
+| `--no-trigger` | off | Skip the R2+X/R2+Y trigger loop and start saqr immediately |
 | `--source` | — | Pass through to saqr (`0` / `realsense` / `/dev/video2` / path) |
 | `--headless` | off | Pass `--headless` to saqr |
 | `--saqr-conf` | — | Pass `--conf` to saqr |
 | `--imgsz` | — | Pass `--imgsz` to saqr |
 | `--device` | — | Pass `--device` to saqr (`cpu` / `0` / `cuda:0`) |
-| `-- <extra>` | — | Everything after `--` is forwarded raw to saqr |
+| `-- <extra>` | — | Everything after `--` is forwarded raw to saqr (use this for `--stream 8080`, `--half`, etc.) |
 
 ### Speaker-id reference
 
@@ -267,20 +326,35 @@ python3 ~/Sanad/voice_example.py 6
 ```
 and pass the new id with `--speaker-id N`.
 
-### What successful output looks like:
+### What a successful run looks like
+
 ```
 [BRIDGE] G1ArmActionClient ready (iface=eth0)
 [BRIDGE] G1 AudioClient ready (speaker_id=2)
-[BRIDGE] launching: /.../python3 -u /home/unitree/Saqr/saqr.py --source realsense --headless
+[BRIDGE] Subscribed to rt/lowstate (wireless remote)
+[BRIDGE] trigger loop ready — press R2+X to start, R2+Y to stop.
+[BRIDGE] R2+X pressed -> start saqr
+[BRIDGE] starting saqr: /.../python3 -u /home/unitree/Saqr/saqr.py --source realsense --headless --stream 8080
+[BRIDGE] tts -> 'Saqr activated.'
 ...
-ID 0001 | NEW | SAFE | wearing: helmet, vest | missing: none | ...
-[BRIDGE] tts -> 'Safe.'
 ID 0002 | NEW | UNSAFE | wearing: none | missing: vest | ...
-[BRIDGE] tts -> 'Not safe! Please wear your protective equipment.'
+[BRIDGE] tts -> 'Please stop. Wear your proper safety equipment. You are missing vest.'
 [BRIDGE] -> reject
 [BRIDGE] -> release arm
+ID 0003 | STATUS_CHANGE | SAFE | wearing: helmet, vest | missing: none | ...
+[BRIDGE] tts -> 'Safe to enter. Have a good day.'
+[BRIDGE] R2+Y pressed -> stop saqr
+[BRIDGE] stopping saqr (SIGINT)
+[BRIDGE] saqr exited rc=-2
+[BRIDGE] tts -> 'Saqr deactivated.'
 ```
 
+> **Note on the SIGINT traceback:** when R2+Y stops saqr, you may see a
+> Python `KeyboardInterrupt` traceback unwinding from inside YOLO. This is
+> expected — saqr.py doesn't catch SIGINT explicitly, so Python prints the
+> stack on its way out. The bridge correctly detects the exit, announces
+> "Saqr deactivated.", and stays alive ready for the next R2+X.
+
 ---
 
 ## Step 5: Check Results (Robot)
@@ -405,7 +479,8 @@ python saqr.py --source realsense --model models/saqr_best.pt --headless \
 | File | Purpose |
 |------|---------|
 | `saqr.py` | Main PPE tracking + detection (RealSense + OpenCV) |
-| `saqr_g1_bridge.py` | Saqr → G1 bridge (onboard TTS + `reject` arm action on UNSAFE/SAFE transitions) |
+| `saqr_g1_bridge.py` | Saqr → G1 bridge (R2+X/R2+Y trigger, onboard TTS + `reject` arm action on UNSAFE/SAFE transitions) |
+| `controller.py` | G1 wireless-remote DDS reader (`LowStateHub`, `combo_r2x()`, `combo_r2y()`); required by the bridge for trigger keys |
 | `detect.py` | Simple detection without tracking |
 | `gui.py` | PySide6 desktop GUI |
 | `manager.py` | Photo management CLI + CSV export |

controller.py

@@ -0,0 +1,101 @@
+"""
+controller.py — G1 wireless-remote DDS reader for Saqr.
+
+Trimmed copy of the helper used by Project/Manual Photographer. The G1's
+built-in remote publishes its button state on the `rt/lowstate` DDS topic;
+LowStateHub subscribes to that topic and exposes the parsed button bits and
+a couple of combo helpers for the bridge to poll.
+
+Buttons follow the same byte layout as Manual Photographer:
+    data1 bits: R1, L1, Start, Select, R2, L2, F1, F3
+    data2 bits: A, B, X, Y, Up, Right, Down, Left
+"""
+
+from __future__ import annotations
+
+import struct
+import time
+from pathlib import Path
+
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+
+
+def auto_pick_iface() -> str:
+    """Best-effort guess of the DDS network interface."""
+    try:
+        net_dir = Path("/sys/class/net")
+        if not net_dir.exists():
+            return "lo"
+        names = [p.name for p in net_dir.iterdir() if p.is_dir()]
+        for pref in ("en", "eth", "wl"):
+            for n in names:
+                if n != "lo" and n.startswith(pref):
+                    return n
+        for n in names:
+            if n != "lo":
+                return n
+        return "lo"
+    except Exception:
+        return "lo"
+
+
+class UnitreeRemote:
+    def __init__(self):
+        self.Lx = 0; self.Rx = 0; self.Ry = 0; self.Ly = 0
+        self.L1 = 0; self.L2 = 0; self.R1 = 0; self.R2 = 0
+        self.A = 0;  self.B = 0;  self.X = 0;  self.Y = 0
+        self.Up = 0; self.Down = 0; self.Left = 0; self.Right = 0
+        self.Select = 0; self.F1 = 0; self.F3 = 0; self.Start = 0
+
+    def _parse_buttons(self, data1: int, data2: int) -> None:
+        self.R1 = (data1 >> 0) & 1; self.L1 = (data1 >> 1) & 1
+        self.Start = (data1 >> 2) & 1; self.Select = (data1 >> 3) & 1
+        self.R2 = (data1 >> 4) & 1; self.L2 = (data1 >> 5) & 1
+        self.F1 = (data1 >> 6) & 1; self.F3 = (data1 >> 7) & 1
+        self.A = (data2 >> 0) & 1; self.B = (data2 >> 1) & 1
+        self.X = (data2 >> 2) & 1; self.Y = (data2 >> 3) & 1
+        self.Up = (data2 >> 4) & 1; self.Right = (data2 >> 5) & 1
+        self.Down = (data2 >> 6) & 1; self.Left = (data2 >> 7) & 1
+
+    def _parse_axes(self, data: bytes) -> None:
+        offsets = [4, 8, 12, 20]  # Lx, Rx, Ry, Ly
+        self.Lx, self.Rx, self.Ry, self.Ly = [
+            struct.unpack('<f', data[o:o + 4])[0] for o in offsets
+        ]
+
+    def parse(self, remote_data: bytes) -> None:
+        self._parse_axes(remote_data)
+        self._parse_buttons(remote_data[2], remote_data[3])
+
+    def state(self) -> dict:
+        return self.__dict__.copy()
+
+
+class LowStateHub:
+    """DDS subscriber callback target. Stores the latest parsed remote state."""
+
+    def __init__(self, watchdog_timeout: float = 0.25):
+        self.low_state: LowState_ | None = None
+        self.first_state = False
+        self.last_state_time = 0.0
+        self.watchdog_timeout = float(watchdog_timeout)
+        self.remote = UnitreeRemote()
+
+    def handler(self, msg: LowState_) -> None:
+        self.low_state = msg
+        self.first_state = True
+        self.last_state_time = time.time()
+        try:
+            self.remote.parse(msg.wireless_remote)
+        except Exception:
+            pass
+
+    def fresh(self) -> bool:
+        return (time.time() - self.last_state_time) < self.watchdog_timeout
+
+    # ── Combo helpers ───────────────────────────────────────────────────────
+    def combo_r2x(self) -> bool:
+        return bool(self.remote.R2 and self.remote.X)
+
+    def combo_r2y(self) -> bool:
+        return bool(self.remote.R2 and self.remote.Y)

saqr_g1_bridge.py

@@ -4,32 +4,34 @@ saqr_g1_bridge.py
 
 Bridge between Saqr PPE detection and the Unitree G1 robot.
 
-Spawns Saqr (saqr.py in this same folder) as a subprocess, parses its event
+Default behavior on the robot: do NOTHING until the operator presses
-stream, and on each per-person status transition:
+**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 "Not safe!" via the G1 onboard TtsMaker (English,
+  * UNSAFE  -> announce missing PPE via the G1 onboard TtsMaker (English,
               speaker_id=2) AND run the 'reject' arm action (id=13).
-  * SAFE    -> announce "Safe!" via the G1 onboard TtsMaker. No arm motion.
+  * SAFE    -> announce "Safe to enter. Have a good day." No arm motion.
   * PARTIAL -> nothing.
 
-Both DDS clients (G1ArmActionClient and G1 AudioClient) share a single
+Both DDS clients (G1ArmActionClient + G1 AudioClient) and the LowState
-ChannelFactoryInitialize call. The TTS speaker_id was identified by running
+subscriber share a single ChannelFactoryInitialize call. The TTS speaker_id
-Project/Sanad/voice_example.py mode 6 — speaker_id=2 is English on current
+was identified by running Project/Sanad/voice_example.py mode 6 — speaker_id=2
-G1 firmware (speaker_id=0 is Chinese regardless of input text).
+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:
-    # default: webcam, default DDS interface
+    # on the robot — wait for R2+X / R2+Y to start/stop Saqr
-    python3 saqr_g1_bridge.py
-
-    # on the robot
     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
+    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
@@ -38,6 +40,8 @@ Usage:
 from __future__ import annotations
 
 import argparse
+import collections
+import datetime
 import os
 import re
 import signal
@@ -46,7 +50,12 @@ import sys
 import threading
 import time
 from pathlib import Path
-from typing import Dict, Optional
+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 ─────────────────────────────────────────────────────────────────
@@ -71,6 +80,35 @@ TTS_UNSAFE_WITH_MISSING  = (
 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(
@@ -121,12 +159,26 @@ class RobotController:
         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)
@@ -162,21 +214,130 @@ class RobotController:
         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:
-            print(f"[BRIDGE] tts -> {text!r}", flush=True)
             code = self.audio_client.TtsMaker(text, self.tts_speaker_id)
-            if code != 0:
-                print(f"[BRIDGE][WARN] TtsMaker return code = {code}", flush=True)
         except Exception as e:
-            print(f"[BRIDGE][ERR] TtsMaker failed: {e}", flush=True)
+            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):
@@ -200,21 +361,131 @@ class RobotController:
 
 # ── 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.last_status: Dict[int, str] = {}
+        self.saqr_args = saqr_args
-        # Per-id cooldown is keyed by (track_id, status) so a SAFE announce
+        self.env = env
-        # and an UNSAFE announce don't share the same timer.
-        self.last_trigger_t: Dict[tuple[int, str], float] = {}
-        self._lock = threading.Lock()
 
+        # 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:
@@ -230,7 +501,7 @@ class Bridge:
         status = m.group("status")
         missing = _parse_list_field(m.group("missing"))
 
-        with self._lock:
+        with self._state_lock:
             prev = self.last_status.get(track_id)
             self.last_status[track_id] = status
 
@@ -260,8 +531,54 @@ class Bridge:
             print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True)
 
 
-# ── Saqr subprocess management ───────────────────────────────────────────────
+# ── Trigger polling loop ─────────────────────────────────────────────────────
-def build_saqr_cmd(saqr_extra_args: list[str]) -> list[str]:
+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).
@@ -294,6 +611,9 @@ def main():
                     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,
@@ -323,53 +643,84 @@ def main():
         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,
     )
-    bridge = Bridge(
-        robot=robot,
-        cooldown_s=args.cooldown,
-        release_after_s=args.release_after,
-    )
-
-    cmd = build_saqr_cmd(saqr_args)
-    print(f"[BRIDGE] launching: {' '.join(cmd)}", flush=True)
-    print(f"[BRIDGE] cwd: {SAQR_DIR}", flush=True)
 
     env = os.environ.copy()
     env["PYTHONUNBUFFERED"] = "1"
 
-    proc = subprocess.Popen(
+    bridge = Bridge(
-        cmd,
+        robot=robot,
-        cwd=str(SAQR_DIR),
+        cooldown_s=args.cooldown,
-        stdout=subprocess.PIPE,
+        release_after_s=args.release_after,
-        stderr=subprocess.STDOUT,
+        saqr_args=saqr_args,
-        bufsize=1,
-        text=True,
         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} -> stopping saqr", flush=True)
+        print(f"[BRIDGE] signal {signum} -> shutting down", flush=True)
-        try:
+        stop_event.set()
-            proc.send_signal(signum)
-        except Exception:
-            pass
 
     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:
-        assert proc.stdout is not None
+        if have_hub:
-        for line in proc.stdout:
+            # Wireless-remote mode: idle until R2+X.
-            bridge.handle_line(line)
+            # 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:
-        rc = proc.wait()
+        # Make sure saqr is stopped before we exit, regardless of mode.
-        print(f"[BRIDGE] saqr exited rc={rc}", flush=True)
+        if bridge.is_running():
-        sys.exit(rc)
+            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__":

start.md

@@ -0,0 +1,186 @@
+# Saqr — Auto-start on boot
+
+How to make `saqr_g1_bridge.py` run automatically on every boot of the
+Unitree G1 (Jetson), via `systemd` + `start_saqr.sh`.
+
+---
+
+## Files involved
+
+| File | Role |
+|------|------|
+| `~/Saqr/saqr_g1_bridge.py` | The bridge process (DDS + TTS + R2+X/R2+Y trigger loop). |
+| `~/Saqr/start_saqr.sh`     | Bash launcher: sources conda, activates `marcus`, `cd ~/Saqr`, exec the bridge with the right flags. |
+| `~/Saqr/saqr-bridge.service` | systemd unit that runs `start_saqr.sh` as user `unitree` on every boot, restarts on failure, logs to journalctl. |
+
+---
+
+## One-time install
+
+Run these on the robot:
+
+```bash
+# 1. Make sure the launcher is executable.
+chmod +x ~/Saqr/start_saqr.sh
+
+# 2. Install the systemd unit system-wide so it starts at BOOT
+#    (not just at login).
+sudo cp ~/Saqr/saqr-bridge.service /etc/systemd/system/
+sudo systemctl daemon-reload
+
+# 3. Enable + start it now.
+sudo systemctl enable --now saqr-bridge
+
+# 4. Verify it came up.
+sudo systemctl status saqr-bridge
+```
+
+You should hear **"Saqr is running. Press R2 plus X to start."** on the
+robot speaker within ~10 seconds. From then on, every reboot auto-starts
+the bridge — no terminal needed.
+
+---
+
+## Daily commands
+
+```bash
+# Follow the live bridge log (replaces the terminal you used to ssh into).
+journalctl -u saqr-bridge -f
+
+# Stop / start / restart on demand.
+sudo systemctl restart saqr-bridge
+sudo systemctl stop saqr-bridge
+sudo systemctl start saqr-bridge
+
+# Disable auto-start at boot (the service stays installed).
+sudo systemctl disable saqr-bridge
+
+# Re-enable auto-start at boot.
+sudo systemctl enable saqr-bridge
+
+# Show the most recent 100 log lines (e.g. after a reboot).
+journalctl -u saqr-bridge -n 100 --no-pager
+
+# Show only this boot's logs.
+journalctl -u saqr-bridge -b
+```
+
+---
+
+## ⚠️ Don't run two bridges at once
+
+Once the systemd service is enabled, the bridge is **already running** in
+the background. If you also run `./start_saqr.sh` in a terminal you'll have
+two bridges fighting over the same DDS clients (you'll see lines like
+`R2+X pressed -> start saqr` immediately followed by
+`start ignored — saqr already running`, because both bridges react to the
+same wireless-remote events).
+
+Pick one mode:
+
+```bash
+# Production: let systemd own the bridge.
+sudo systemctl start saqr-bridge
+journalctl -u saqr-bridge -f
+
+# Dev / debugging: stop the systemd one first, then run by hand.
+sudo systemctl stop saqr-bridge
+~/Saqr/start_saqr.sh
+```
+
+---
+
+## Quick reboot test
+
+```bash
+sudo reboot
+
+# After the robot is back up:
+ssh unitree@192.168.123.164
+sudo systemctl status saqr-bridge     # should be "active (running)"
+journalctl -u saqr-bridge -n 50       # boot log including the
+                                      # "Saqr is running" TTS line
+```
+
+---
+
+## Updating the bridge / launcher / unit
+
+After editing any of the three files:
+
+```bash
+# If you changed start_saqr.sh or saqr_g1_bridge.py:
+sudo systemctl restart saqr-bridge
+
+# If you changed saqr-bridge.service itself:
+sudo cp ~/Saqr/saqr-bridge.service /etc/systemd/system/
+sudo systemctl daemon-reload
+sudo systemctl restart saqr-bridge
+```
+
+---
+
+## Configuration overrides
+
+`start_saqr.sh` reads its config from environment variables, so you can
+override any of them without editing the script. Defaults:
+
+| Variable      | Default          | Meaning |
+|---------------|------------------|---------|
+| `SAQR_DIR`    | `$HOME/Saqr`     | Where `saqr_g1_bridge.py` lives. |
+| `CONDA_ROOT`  | `$HOME/miniconda3` | Miniconda install path. |
+| `CONDA_ENV`   | `marcus`         | Conda env that has `unitree_sdk2py`, `ultralytics`, `pyrealsense2`. |
+| `DDS_IFACE`   | `eth0`           | DDS network interface for the G1. |
+| `SAQR_SOURCE` | `realsense`      | `--source` passed to saqr (`realsense` / `/dev/video2` / `0`). |
+| `STREAM_PORT` | `8080`           | MJPEG stream port (`-- --stream $STREAM_PORT`). |
+
+To override permanently in the systemd service, add `Environment=` lines
+to `/etc/systemd/system/saqr-bridge.service` and run
+`sudo systemctl daemon-reload && sudo systemctl restart saqr-bridge`.
+Example:
+
+```ini
+Environment=SAQR_SOURCE=/dev/video2
+Environment=STREAM_PORT=9090
+```
+
+---
+
+## Troubleshooting
+
+### Service won't start
+
+```bash
+sudo systemctl status saqr-bridge
+journalctl -u saqr-bridge -n 100 --no-pager
+```
+
+Common causes:
+- `start_saqr.sh` not executable → `chmod +x ~/Saqr/start_saqr.sh`
+- conda env name wrong → check `CONDA_ENV`
+- `unitree_sdk2py` missing in the env → run `~/Saqr/start_saqr.sh` by hand to see the import error
+- DDS interface wrong → set `DDS_IFACE=enp...` if the G1 isn't on `eth0`
+
+### "No device connected" when pressing R2+X
+
+The RealSense USB hiccup. The bridge stays alive and announces
+**"Camera not connected. Please plug in the camera and try again."** —
+just unplug/replug the camera and press R2+X again. If it persists,
+fall back to the V4L2 path:
+
+```bash
+sudo systemctl edit saqr-bridge
+# add:
+[Service]
+Environment=SAQR_SOURCE=/dev/video2
+# save, then:
+sudo systemctl restart saqr-bridge
+```
+
+### Bridge is running twice
+
+```bash
+ps -ef | grep saqr_g1_bridge
+# If you see two python processes, kill the manual one and let systemd own it:
+sudo systemctl restart saqr-bridge
+```

start_saqr.sh

@@ -0,0 +1,70 @@
+#!/bin/bash
+# ============================================================================
+#  start_saqr.sh — boot launcher for the Saqr/G1 bridge.
+# ============================================================================
+#
+#  What it does:
+#    1. Sources miniconda and activates the `marcus` env (which has
+#       unitree_sdk2py + ultralytics + pyrealsense2 installed).
+#    2. cd ~/Saqr
+#    3. Execs saqr_g1_bridge.py with the production flags. The bridge will:
+#         - init the G1 arm + audio + LowState DDS clients
+#         - announce "Saqr is running. Press R2 plus X to start." via TtsMaker
+#         - sit idle until R2+X is pressed
+#         - if R2+X is pressed but no camera is plugged, announce
+#           "Camera not connected. Please plug in the camera and try again."
+#           and stay idle ready for the next press
+#
+#  Designed to be run by systemd at boot — see saqr-bridge.service.
+#  Can also be run manually:  ~/Saqr/start_saqr.sh
+# ============================================================================
+
+set -u
+
+# ── Config (override via env if needed) ──────────────────────────────────────
+SAQR_DIR="${SAQR_DIR:-$HOME/Saqr}"
+CONDA_ROOT="${CONDA_ROOT:-$HOME/miniconda3}"
+CONDA_ENV="${CONDA_ENV:-marcus}"
+DDS_IFACE="${DDS_IFACE:-eth0}"
+SAQR_SOURCE="${SAQR_SOURCE:-realsense}"
+STREAM_PORT="${STREAM_PORT:-8080}"
+
+# ── Sanity checks ────────────────────────────────────────────────────────────
+if [ ! -d "$SAQR_DIR" ]; then
+    echo "[start_saqr] FATAL: SAQR_DIR not found: $SAQR_DIR" >&2
+    exit 1
+fi
+
+if [ ! -f "$SAQR_DIR/saqr_g1_bridge.py" ]; then
+    echo "[start_saqr] FATAL: saqr_g1_bridge.py not found in $SAQR_DIR" >&2
+    exit 1
+fi
+
+if [ ! -f "$CONDA_ROOT/etc/profile.d/conda.sh" ]; then
+    echo "[start_saqr] FATAL: conda not found at $CONDA_ROOT" >&2
+    exit 1
+fi
+
+# ── Activate conda ───────────────────────────────────────────────────────────
+# shellcheck disable=SC1091
+source "$CONDA_ROOT/etc/profile.d/conda.sh"
+conda activate "$CONDA_ENV" || {
+    echo "[start_saqr] FATAL: failed to activate conda env: $CONDA_ENV" >&2
+    exit 1
+}
+
+cd "$SAQR_DIR" || {
+    echo "[start_saqr] FATAL: cd $SAQR_DIR failed" >&2
+    exit 1
+}
+
+echo "[start_saqr] env=$CONDA_ENV cwd=$PWD iface=$DDS_IFACE source=$SAQR_SOURCE stream=$STREAM_PORT"
+echo "[start_saqr] launching bridge..."
+
+# Exec so this script's PID is replaced by the bridge — systemd then
+# tracks the bridge directly and signals reach Python correctly.
+exec python3 saqr_g1_bridge.py \
+    --iface "$DDS_IFACE" \
+    --source "$SAQR_SOURCE" \
+    --headless \
+    -- --stream "$STREAM_PORT"