397 lines
13 KiB
Python
397 lines
13 KiB
Python
"""Bridge between Saqr PPE detection and the Unitree G1 robot.
|
|
|
|
Default behavior on the robot: do NOTHING until the operator presses
|
|
**R2+X** on the G1 wireless remote. R2+X starts saqr as a subprocess,
|
|
R2+Y stops it. While saqr is running the bridge parses its event stream and:
|
|
|
|
* UNSAFE -> announce missing PPE via the G1 onboard TtsMaker (English,
|
|
speaker_id=2) AND run the 'reject' arm action (id=13).
|
|
* SAFE -> announce "Safe to enter. Have a good day." No arm motion.
|
|
* PARTIAL -> nothing.
|
|
|
|
See docs/DEPLOY.md for wireless-remote workflow and systemd deploy notes.
|
|
|
|
Saqr event line format (from saqr.core.events.emit_event):
|
|
ID 0001 | NEW | UNSAFE | wearing: ... | missing: ... | unknown: ...
|
|
ID 0001 | STATUS_CHANGE | SAFE | wearing: ... | missing: ... | unknown: ...
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import os
|
|
import re
|
|
import signal
|
|
import subprocess
|
|
import sys
|
|
import threading
|
|
import time
|
|
from typing import Dict, Optional
|
|
|
|
from saqr.core.paths import PROJECT_ROOT
|
|
from saqr.robot.robot_controller import RobotController
|
|
|
|
DANGER_STATUS = "UNSAFE"
|
|
SAFE_STATUS = "SAFE"
|
|
|
|
# speaker_id=2 was confirmed English on current G1 firmware via
|
|
# Project/Sanad/voice_example.py mode 6. speaker_id=0 is Chinese.
|
|
TTS_SPEAKER_ID = 2
|
|
|
|
TTS_TEXT_SAFE = "Safe to enter. Have a good day."
|
|
TTS_UNSAFE_WITH_MISSING = (
|
|
"Please stop. Wear your proper safety equipment. You are missing {items}."
|
|
)
|
|
TTS_UNSAFE_GENERIC = "Please stop. Wear your proper safety equipment."
|
|
TTS_BRIDGE_DEACTIVATED = "Saqr deactivated."
|
|
TTS_BRIDGE_READY = "Saqr is running. Press R2 plus X to start."
|
|
TTS_BRIDGE_NO_CAMERA = (
|
|
"Camera not connected. Please plug in the camera and try again."
|
|
)
|
|
|
|
QUICK_FAIL_WINDOW_S = 8.0
|
|
|
|
# ID NNNN | EVENT_TYPE | STATUS | wearing: ... | missing: ... | unknown: ...
|
|
EVENT_RE = re.compile(
|
|
r"^ID\s+(?P<id>\d+)\s*\|\s*"
|
|
r"(?P<event>NEW|STATUS_CHANGE)\s*\|\s*"
|
|
r"(?P<status>SAFE|PARTIAL|UNSAFE)\s*\|\s*"
|
|
r"wearing:\s*(?P<wearing>[^|]*?)\s*\|\s*"
|
|
r"missing:\s*(?P<missing>[^|]*?)\s*\|\s*"
|
|
r"unknown:\s*(?P<unknown>.*?)\s*$"
|
|
)
|
|
|
|
|
|
def _parse_list_field(s: str) -> list:
|
|
s = (s or "").strip()
|
|
if not s or s.lower() == "none":
|
|
return []
|
|
return [x.strip() for x in s.split(",") if x.strip()]
|
|
|
|
|
|
def _human_join(items: list) -> str:
|
|
if not items:
|
|
return ""
|
|
if len(items) == 1:
|
|
return items[0]
|
|
if len(items) == 2:
|
|
return f"{items[0]} and {items[1]}"
|
|
return ", ".join(items[:-1]) + f", and {items[-1]}"
|
|
|
|
|
|
def build_unsafe_tts(missing: list) -> str:
|
|
if not missing:
|
|
return TTS_UNSAFE_GENERIC
|
|
return TTS_UNSAFE_WITH_MISSING.format(items=_human_join(missing))
|
|
|
|
|
|
def build_saqr_cmd(saqr_extra_args: list) -> list:
|
|
"""Invoke the saqr CLI via ``python -m`` so it picks up the package layout."""
|
|
return [sys.executable, "-u", "-m", "saqr.apps.saqr_cli", *saqr_extra_args]
|
|
|
|
|
|
def split_argv(argv):
|
|
if "--" in argv:
|
|
idx = argv.index("--")
|
|
return argv[:idx], argv[idx + 1:]
|
|
return argv, []
|
|
|
|
|
|
class Bridge:
|
|
"""Owns the saqr 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], cwd: str):
|
|
self.robot = robot
|
|
self.cooldown_s = cooldown_s
|
|
self.release_after_s = release_after_s
|
|
self.saqr_args = saqr_args
|
|
self.env = env
|
|
self.cwd = cwd
|
|
|
|
self.last_status: Dict[int, str] = {}
|
|
self.last_trigger_t: Dict[tuple, float] = {}
|
|
self._state_lock = threading.Lock()
|
|
|
|
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
|
|
|
|
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=self.cwd,
|
|
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()
|
|
|
|
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
|
|
|
|
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 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)
|
|
|
|
def handle_line(self, line: str):
|
|
line = line.rstrip()
|
|
if not line:
|
|
return
|
|
print(line, flush=True)
|
|
|
|
m = EVENT_RE.match(line)
|
|
if not m:
|
|
return
|
|
|
|
track_id = int(m.group("id"))
|
|
status = m.group("status")
|
|
missing = _parse_list_field(m.group("missing"))
|
|
|
|
with self._state_lock:
|
|
prev = self.last_status.get(track_id)
|
|
self.last_status[track_id] = status
|
|
|
|
if status not in (DANGER_STATUS, SAFE_STATUS):
|
|
return
|
|
|
|
if prev == status:
|
|
return
|
|
|
|
now = time.time()
|
|
last_t = self.last_trigger_t.get((track_id, status), 0.0)
|
|
if (now - last_t) < self.cooldown_s:
|
|
return
|
|
self.last_trigger_t[(track_id, status)] = now
|
|
|
|
try:
|
|
if status == DANGER_STATUS:
|
|
self.robot.speak(build_unsafe_tts(missing))
|
|
self.robot.reject(release_after=self.release_after_s)
|
|
else:
|
|
self.robot.speak(TTS_TEXT_SAFE)
|
|
except Exception as e:
|
|
print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True)
|
|
|
|
|
|
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)."""
|
|
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()
|
|
|
|
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)
|
|
|
|
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)
|
|
|
|
|
|
def main():
|
|
bridge_argv, saqr_extra = split_argv(sys.argv[1:])
|
|
|
|
ap = argparse.ArgumentParser(
|
|
description="Bridge Saqr PPE events to the G1 arm 'reject' action."
|
|
)
|
|
ap.add_argument("--iface", default=None,
|
|
help="DDS network interface (e.g. eth0).")
|
|
ap.add_argument("--timeout", type=float, default=10.0)
|
|
ap.add_argument("--cooldown", type=float, default=8.0)
|
|
ap.add_argument("--release-after", type=float, default=2.0)
|
|
ap.add_argument("--dry-run", action="store_true")
|
|
ap.add_argument("--speaker-id", type=int, default=TTS_SPEAKER_ID)
|
|
ap.add_argument("--no-trigger", action="store_true")
|
|
|
|
ap.add_argument("--source", default=None)
|
|
ap.add_argument("--headless", action="store_true")
|
|
ap.add_argument("--saqr-conf", type=float, default=None)
|
|
ap.add_argument("--imgsz", type=int, default=None)
|
|
ap.add_argument("--device", default=None)
|
|
|
|
args = ap.parse_args(bridge_argv)
|
|
|
|
saqr_args: list = []
|
|
if args.source is not None:
|
|
saqr_args += ["--source", args.source]
|
|
if args.headless:
|
|
saqr_args += ["--headless"]
|
|
if args.saqr_conf is not None:
|
|
saqr_args += ["--conf", str(args.saqr_conf)]
|
|
if args.imgsz is not None:
|
|
saqr_args += ["--imgsz", str(args.imgsz)]
|
|
if args.device is not None:
|
|
saqr_args += ["--device", args.device]
|
|
saqr_args += saqr_extra
|
|
|
|
use_trigger = not args.no_trigger and not args.dry_run
|
|
|
|
robot = RobotController(
|
|
iface=args.iface,
|
|
timeout=args.timeout,
|
|
dry_run=args.dry_run,
|
|
tts_speaker_id=args.speaker_id,
|
|
want_lowstate=use_trigger,
|
|
)
|
|
|
|
env = os.environ.copy()
|
|
env["PYTHONUNBUFFERED"] = "1"
|
|
|
|
bridge = Bridge(
|
|
robot=robot,
|
|
cooldown_s=args.cooldown,
|
|
release_after_s=args.release_after,
|
|
saqr_args=saqr_args,
|
|
env=env,
|
|
cwd=str(PROJECT_ROOT),
|
|
)
|
|
print(f"[BRIDGE] saqr cmd template: {' '.join(build_saqr_cmd(saqr_args))}",
|
|
flush=True)
|
|
print(f"[BRIDGE] cwd: {PROJECT_ROOT}", flush=True)
|
|
|
|
stop_event = threading.Event()
|
|
|
|
def _forward_signal(signum, _frame):
|
|
print(f"[BRIDGE] signal {signum} -> shutting down", flush=True)
|
|
stop_event.set()
|
|
|
|
signal.signal(signal.SIGINT, _forward_signal)
|
|
signal.signal(signal.SIGTERM, _forward_signal)
|
|
|
|
have_hub = use_trigger and robot.hub is not None
|
|
if use_trigger and not have_hub:
|
|
print("[BRIDGE][WARN] --no-trigger not set, but no LowState hub is "
|
|
"available. Falling back to legacy auto-start mode.", flush=True)
|
|
|
|
trigger_thread: Optional[threading.Thread] = None
|
|
try:
|
|
if have_hub:
|
|
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()
|
|
|
|
while not stop_event.is_set():
|
|
time.sleep(0.2)
|
|
else:
|
|
bridge.start_saqr()
|
|
while not stop_event.is_set() and bridge.is_running():
|
|
time.sleep(0.2)
|
|
finally:
|
|
if bridge.is_running():
|
|
bridge.stop_saqr()
|
|
stop_event.set()
|
|
if trigger_thread is not None:
|
|
trigger_thread.join(timeout=1.0)
|
|
try:
|
|
robot.shutdown_tts()
|
|
except Exception:
|
|
pass
|
|
print("[BRIDGE] bye.", flush=True)
|
|
sys.exit(0)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|