kassam/Saqr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update 2026-04-15 11:14:11

Browse Source
This commit is contained in:
kassam 2026-04-15 11:14:11 +04:00
parent 79873d79f7
commit 7cd8d78c3e
5 changed files with 861 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
DEPLOY.md
View File

@ -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`) |
| `-- <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 |

101
controller.py Normal file
View File

@ -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)

457
saqr_g1_bridge.py
View File

@ -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__":

186
start.md Normal file
View File

@ -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
```

70
start_saqr.sh Executable file
View File

@ -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"