diff --git a/DEPLOY.md b/DEPLOY.md index 26a191e..769643f 100644 --- a/DEPLOY.md +++ b/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 -G1 **onboard TTS** and the G1 **arm action client** on each per-person status -transition: +`saqr_g1_bridge.py` is the production entry point. It does **not** run Saqr +itself — it sits idle, watches the G1 wireless remote, spawns `saqr.py` as a +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` | -| → SAFE | "Safe." | — | +| → 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 to enter. Have a good day." | — | | → PARTIAL | — | — | -Requires `unitree_sdk2py` installed on the robot and a reachable DDS bus on -`eth0`. The bridge uses a single `ChannelFactoryInitialize` for both clients. +The missing-item list is parsed live from Saqr's event line and joined in +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) | -| `--cooldown` | `8.0` | Per-(id, status) seconds before re-triggering | +| `--timeout` | `10.0` | Arm/Audio/LowState client timeout (seconds) | +| `--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`) | -| `-- ` | — | Everything after `--` is forwarded raw to saqr | +| `-- ` | — | 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 | diff --git a/controller.py b/controller.py new file mode 100644 index 0000000..65e554c --- /dev/null +++ b/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(' 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) diff --git a/saqr_g1_bridge.py b/saqr_g1_bridge.py index f95089b..f1b5a7d 100644 --- a/saqr_g1_bridge.py +++ b/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 -stream, and on each per-person status transition: +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 "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 -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). +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: - # default: webcam, default DDS interface - python3 saqr_g1_bridge.py - - # on the robot + # 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 + 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] = {} - # Per-id cooldown is keyed by (track_id, status) so a SAFE announce - # and an UNSAFE announce don't share the same timer. - self.last_trigger_t: Dict[tuple[int, str], float] = {} - self._lock = threading.Lock() + 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: @@ -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 ─────────────────────────────────────────────── -def build_saqr_cmd(saqr_extra_args: list[str]) -> list[str]: +# ── 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). @@ -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( - cmd, - cwd=str(SAQR_DIR), - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - bufsize=1, - text=True, + 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} -> stopping saqr", flush=True) - try: - proc.send_signal(signum) - except Exception: - pass + 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: - assert proc.stdout is not None - for line in proc.stdout: - bridge.handle_line(line) + 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: - rc = proc.wait() - print(f"[BRIDGE] saqr exited rc={rc}", flush=True) - sys.exit(rc) + # 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__": diff --git a/start.md b/start.md new file mode 100644 index 0000000..55e16e8 --- /dev/null +++ b/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 +``` diff --git a/start_saqr.sh b/start_saqr.sh new file mode 100755 index 0000000..1c7b039 --- /dev/null +++ b/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"