422 lines
14 KiB
Python
422 lines
14 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 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 core.paths import PROJECT_ROOT
|
|
from robot.robot_controller import RobotController
|
|
from utils.config import load_config
|
|
|
|
import datetime
|
|
|
|
|
|
def _ts() -> str:
|
|
return datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3]
|
|
|
|
_ROBOT = load_config("robot")
|
|
_BRIDGE = _ROBOT["bridge"]
|
|
_PHRASES = _ROBOT["phrases"]
|
|
|
|
DANGER_STATUS = "UNSAFE"
|
|
SAFE_STATUS = "SAFE"
|
|
|
|
# speaker_id is locked to a language by G1 firmware: 2=English, 0=Chinese.
|
|
# Confirmed via Project/Sanad/voice_example.py mode 6.
|
|
TTS_SPEAKER_ID = _ROBOT["tts"]["speaker_id"]
|
|
|
|
TTS_TEXT_SAFE = _PHRASES["safe"]
|
|
TTS_UNSAFE_WITH_MISSING = _PHRASES["unsafe_missing"]
|
|
TTS_UNSAFE_GENERIC = _PHRASES["unsafe_generic"]
|
|
TTS_BRIDGE_DEACTIVATED = _PHRASES["deactivated"]
|
|
TTS_BRIDGE_READY = _PHRASES["ready"]
|
|
TTS_BRIDGE_NO_CAMERA = _PHRASES["no_camera"]
|
|
|
|
QUICK_FAIL_WINDOW_S = _BRIDGE["quick_fail_window"]
|
|
TRIGGER_POLL_HZ = _BRIDGE["trigger_poll_hz"]
|
|
|
|
# [HH:MM:SS.fff] ID NNNN | EVENT_TYPE | STATUS | wearing: ... | missing: ... | unknown: ...
|
|
# The leading timestamp is optional for backwards compatibility with old logs.
|
|
EVENT_RE = re.compile(
|
|
r"^(?:\[[\d:.]+\]\s+)?"
|
|
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", "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,
|
|
audio_lead_s: float = 0.3):
|
|
self.robot = robot
|
|
self.cooldown_s = cooldown_s
|
|
self.release_after_s = release_after_s
|
|
self.audio_lead_s = audio_lead_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(f"[BRIDGE {_ts()}] start ignored — saqr already running", flush=True)
|
|
return
|
|
|
|
cmd = build_saqr_cmd(self.saqr_args)
|
|
print(f"[BRIDGE {_ts()}] 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(f"[BRIDGE {_ts()}] stop ignored — saqr not running", flush=True)
|
|
self.proc = None
|
|
return
|
|
print(f"[BRIDGE {_ts()}] stopping saqr (SIGINT)", flush=True)
|
|
try:
|
|
proc.send_signal(signal.SIGINT)
|
|
except Exception:
|
|
pass
|
|
|
|
try:
|
|
proc.wait(timeout=3.0)
|
|
except subprocess.TimeoutExpired:
|
|
print(f"[BRIDGE {_ts()}] saqr did not exit in 3s, sending SIGTERM", flush=True)
|
|
try:
|
|
proc.terminate()
|
|
proc.wait(timeout=2.0)
|
|
except subprocess.TimeoutExpired:
|
|
print(f"[BRIDGE {_ts()}] 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, category="fixed", key="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 {_ts()}][ERR] reader thread: {e}", flush=True)
|
|
rc = proc.wait()
|
|
lifetime = time.time() - start_t if start_t > 0 else 0.0
|
|
print(f"[BRIDGE {_ts()}] 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, category="fixed", key="no_camera")
|
|
except Exception as e:
|
|
print(f"[BRIDGE {_ts()}][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:
|
|
# Fire audio first, give the worker thread a head start so
|
|
# PlayStream reaches the firmware BEFORE the arm command.
|
|
# Once audio is actively playing, the arm command queues
|
|
# behind it without blocking playback → audible overlap.
|
|
if missing:
|
|
key = "_".join(sorted(missing))
|
|
self.robot.speak(build_unsafe_tts(missing),
|
|
category="unsafe_missing", key=key)
|
|
else:
|
|
self.robot.speak(TTS_UNSAFE_GENERIC,
|
|
category="fixed", key="unsafe_generic")
|
|
time.sleep(self.audio_lead_s)
|
|
self.robot.reject(release_after=self.release_after_s)
|
|
else:
|
|
self.robot.speak(TTS_TEXT_SAFE,
|
|
category="fixed", key="safe")
|
|
except Exception as e:
|
|
print(f"[BRIDGE {_ts()}][ERR] robot action failed: {e}", flush=True)
|
|
|
|
|
|
def trigger_loop(bridge: Bridge, hub, stop_event: threading.Event,
|
|
poll_hz: float = TRIGGER_POLL_HZ):
|
|
"""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(f"[BRIDGE {_ts()}] 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(f"[BRIDGE {_ts()}] R2+X pressed -> start saqr", flush=True)
|
|
try:
|
|
bridge.start_saqr()
|
|
except Exception as e:
|
|
print(f"[BRIDGE {_ts()}][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(f"[BRIDGE {_ts()}] R2+Y pressed -> stop saqr", flush=True)
|
|
try:
|
|
bridge.stop_saqr()
|
|
except Exception as e:
|
|
print(f"[BRIDGE {_ts()}][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=_BRIDGE["iface"],
|
|
help="DDS network interface (e.g. eth0).")
|
|
ap.add_argument("--timeout", type=float, default=_BRIDGE["timeout"])
|
|
ap.add_argument("--cooldown", type=float, default=_BRIDGE["cooldown"])
|
|
ap.add_argument("--release-after", type=float, default=_BRIDGE["release_after"])
|
|
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,
|
|
audio_lead_s=float(_BRIDGE.get("audio_lead_s", 0.3)),
|
|
saqr_args=saqr_args,
|
|
env=env,
|
|
cwd=str(PROJECT_ROOT),
|
|
)
|
|
print(f"[BRIDGE {_ts()}] saqr cmd template: {' '.join(build_saqr_cmd(saqr_args))}",
|
|
flush=True)
|
|
print(f"[BRIDGE {_ts()}] cwd: {PROJECT_ROOT}", flush=True)
|
|
|
|
stop_event = threading.Event()
|
|
|
|
def _forward_signal(signum, _frame):
|
|
print(f"[BRIDGE {_ts()}] 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(f"[BRIDGE {_ts()}][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, category="fixed", key="ready")
|
|
except Exception as e:
|
|
print(f"[BRIDGE {_ts()}][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(f"[BRIDGE {_ts()}] bye.", flush=True)
|
|
sys.exit(0)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|