From a94d20ab15a346ea88437f244a8609496d4a2f9c Mon Sep 17 00:00:00 2001 From: kassam Date: Sun, 19 Apr 2026 23:50:50 +0400 Subject: [PATCH] Update 2026-04-19 23:50:47 --- .gitignore | 28 +- DEPLOY.md | 564 ----------- README.md | 51 + {Config => config}/logging.json | 0 deploy.sh | 129 --- docs/DEPLOY.md | 322 +++++++ docs/start.md | 169 ++++ .../use_case_catalogue.pdf | Bin pyproject.toml | 31 + run_local.sh | 110 --- run_robot.sh | 116 --- saqr.py | 909 ------------------ saqr/__init__.py | 3 + saqr/apps/__init__.py | 0 detect.py => saqr/apps/detect_cli.py | 56 +- manager.py => saqr/apps/manager_cli.py | 63 +- saqr/apps/saqr_cli.py | 193 ++++ train.py => saqr/apps/train_cli.py | 51 +- view_stream.py => saqr/apps/view_stream.py | 18 +- saqr/core/__init__.py | 0 saqr/core/camera.py | 88 ++ saqr/core/capture.py | 41 + saqr/core/compliance.py | 36 + saqr/core/detection.py | 56 ++ saqr/core/drawing.py | 64 ++ saqr/core/events.py | 93 ++ saqr/core/geometry.py | 35 + saqr/core/grouping.py | 80 ++ saqr/core/model.py | 20 + saqr/core/paths.py | 19 + saqr/core/pipeline.py | 40 + saqr/core/streaming.py | 62 ++ saqr/core/tracking.py | 118 +++ saqr/gui/__init__.py | 0 gui.py => saqr/gui/app.py | 131 +-- saqr/robot/__init__.py | 0 saqr/robot/bridge.py | 396 ++++++++ controller.py => saqr/robot/controller.py | 0 saqr/robot/robot_controller.py | 216 +++++ saqr/utils/__init__.py | 0 logger.py => saqr/utils/logger.py | 11 +- saqr_g1_bridge.py | 727 -------------- scripts/deploy.sh | 106 ++ scripts/run_local.sh | 87 ++ scripts/run_robot.sh | 102 ++ scripts/saqr-bridge.service | 22 + scripts/start_saqr.sh | 65 ++ start.md | 186 ---- start_saqr.sh | 70 -- 49 files changed, 2652 insertions(+), 3032 deletions(-) delete mode 100644 DEPLOY.md create mode 100644 README.md rename {Config => config}/logging.json (100%) delete mode 100755 deploy.sh create mode 100644 docs/DEPLOY.md create mode 100644 docs/start.md rename use case catalogue.pdf => docs/use_case_catalogue.pdf (100%) create mode 100644 pyproject.toml delete mode 100755 run_local.sh delete mode 100755 run_robot.sh delete mode 100644 saqr.py create mode 100644 saqr/__init__.py create mode 100644 saqr/apps/__init__.py rename detect.py => saqr/apps/detect_cli.py (70%) rename manager.py => saqr/apps/manager_cli.py (81%) create mode 100644 saqr/apps/saqr_cli.py rename train.py => saqr/apps/train_cli.py (70%) rename view_stream.py => saqr/apps/view_stream.py (66%) create mode 100644 saqr/core/__init__.py create mode 100644 saqr/core/camera.py create mode 100644 saqr/core/capture.py create mode 100644 saqr/core/compliance.py create mode 100644 saqr/core/detection.py create mode 100644 saqr/core/drawing.py create mode 100644 saqr/core/events.py create mode 100644 saqr/core/geometry.py create mode 100644 saqr/core/grouping.py create mode 100644 saqr/core/model.py create mode 100644 saqr/core/paths.py create mode 100644 saqr/core/pipeline.py create mode 100644 saqr/core/streaming.py create mode 100644 saqr/core/tracking.py create mode 100644 saqr/gui/__init__.py rename gui.py => saqr/gui/app.py (76%) create mode 100644 saqr/robot/__init__.py create mode 100644 saqr/robot/bridge.py rename controller.py => saqr/robot/controller.py (100%) create mode 100644 saqr/robot/robot_controller.py create mode 100644 saqr/utils/__init__.py rename logger.py => saqr/utils/logger.py (85%) delete mode 100644 saqr_g1_bridge.py create mode 100755 scripts/deploy.sh create mode 100755 scripts/run_local.sh create mode 100755 scripts/run_robot.sh create mode 100644 scripts/saqr-bridge.service create mode 100755 scripts/start_saqr.sh delete mode 100644 start.md delete mode 100755 start_saqr.sh diff --git a/.gitignore b/.gitignore index 77fb19b..f02b40a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,10 +1,26 @@ -dataset/ -runs/ -models/ -captures/ -Logs/ +# Python __pycache__/ *.pyc +*.pyo +*.egg-info/ +.venv/ +venv/ + +# Logs +*.log +logs/ +Logs/ + +# ML training outputs & datasets (too large for git) +data/ +runtime/ *.pt *.pth -*.log +*.onnx +*.engine +*.weights + +# OS / editor +.DS_Store +.vscode/ +.idea/ diff --git a/DEPLOY.md b/DEPLOY.md deleted file mode 100644 index 769643f..0000000 --- a/DEPLOY.md +++ /dev/null @@ -1,564 +0,0 @@ -# Saqr PPE Detection - Deployment Guide -## Unitree G1 Robot + Intel RealSense D435I - ---- - -## Robot Details - -| Item | Value | -|------|-------| -| Robot | Unitree G1 Humanoid | -| IP | `192.168.123.164` | -| User | `unitree` | -| OS | Ubuntu 20.04 (aarch64 / Jetson) | -| Python | 3.10 (conda env: `teleimager`) | -| Camera | Intel RealSense D435I | -| Serial | `243622073459` | -| Port | USB 3.2 @ `/dev/video0` | - ---- - -## Step 1: Train the Model (Dev Machine) - -```bash -cd ~/Robotics_workspace/AI/Saqr -conda activate AI_MSI_yolo -python train.py --dataset dataset --epochs 100 --batch 16 -``` - -Verify model exists: -```bash -ls -lh models/saqr_best.pt -# Expected: ~5.3 MB -``` - ---- - -## Step 2: Deploy to Robot (Dev Machine) - -### Option A: Auto deploy -```bash -cd ~/Robotics_workspace/AI/Saqr -./deploy.sh -``` - -### Option B: Manual SCP -```bash -# Create folders -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 controller.py detect.py manager.py logger.py gui.py requirements.txt deploy.sh DEPLOY.md \ - unitree@192.168.123.164:~/Saqr/ - -# Copy config -scp Config/logging.json unitree@192.168.123.164:~/Saqr/Config/ - -# Copy trained model (5.3 MB) -scp models/saqr_best.pt unitree@192.168.123.164:~/Saqr/models/ -``` - ---- - -## Step 3: Install Dependencies (Robot) - -```bash -ssh unitree@192.168.123.164 -``` - -### Fix system clock (required for SSL/pip): -```bash -sudo date -s "2026-04-10 15:00:00" -``` - -### Install into teleimager conda env: -```bash -conda activate teleimager -python -m pip install ultralytics opencv-python-headless numpy PyYAML -``` - -If pip fails (SSL errors), install offline from dev machine: -```bash -# On dev machine: -mkdir -p /tmp/saqr_pkgs -pip download ultralytics opencv-python-headless numpy PyYAML \ - -d /tmp/saqr_pkgs --python-version 3.10 --platform manylinux2014_aarch64 --only-binary=:all: -scp -r /tmp/saqr_pkgs unitree@192.168.123.164:/tmp/saqr_pkgs - -# On robot: -conda activate teleimager -python -m pip install --no-index --find-links=/tmp/saqr_pkgs ultralytics opencv-python-headless numpy PyYAML -``` - -### Install Jetson GPU PyTorch (for CUDA acceleration): -```bash -# Remove pip PyTorch (wrong CUDA version) -python -m pip uninstall torch torchvision -y - -# Install Jetson-specific PyTorch for JetPack 5.1 / CUDA 11.4 -python -m pip install --no-cache-dir \ - https://developer.download.nvidia.com/compute/redist/jp/v51/pytorch/torch-2.1.0a0+41361538.nv23.06-cp310-cp310-linux_aarch64.whl - -python -m pip install --no-cache-dir \ - https://developer.download.nvidia.com/compute/redist/jp/v51/pytorch/torchvision-0.16.1a0+5e8e2f1-cp310-cp310-linux_aarch64.whl -``` - -### Fix Qt / Display (choose one): - -**A) At the robot's physical terminal (monitor connected):** -```bash -xhost +local: -export DISPLAY=:0 -export QT_QPA_PLATFORM=xcb -``` - -**B) Via SSH with X11 forwarding:** -```bash -# From dev machine: -ssh -X unitree@192.168.123.164 -export QT_QPA_PLATFORM=xcb -``` - -**C) Headless / no display (SSH without -X):** -```bash -export QT_QPA_PLATFORM=offscreen -# Always add --headless flag when running saqr.py -``` - -**Make permanent:** -```bash -echo 'export QT_QPA_PLATFORM=offscreen' >> ~/.bashrc -source ~/.bashrc -``` - -**Common error:** `Invalid MIT-MAGIC-COOKIE-1 key` or `could not connect to display :0` -This means you're in SSH without X11 auth. Either use `ssh -X`, run `xhost +local:` on the physical terminal, or switch to headless mode. - -### Fix system clock (required for pip/SSL): -```bash -sudo date -s "2026-04-10 16:00:00" -``` - -### Verify install: -```bash -python -c "from ultralytics import YOLO; print('ultralytics OK')" -python -c "import torch; print('CUDA:', torch.cuda.is_available())" -python -c "import cv2; print('opencv OK')" -``` - ---- - -## Step 4: Run PPE Detection (Robot) - -### Option A: OpenCV + RealSense RGB (recommended, no pyrealsense2 needed): -```bash -conda activate teleimager -cd ~/Saqr - -# === WITH DISPLAY (physical monitor on robot) === -xhost + -export DISPLAY=:0 -python saqr.py --source /dev/video2 --model models/saqr_best.pt - -# === HEADLESS via SSH (no display, saves captures + CSV) === -export QT_QPA_PLATFORM=offscreen -python saqr.py --source /dev/video2 --model models/saqr_best.pt --headless -``` - -**Note:** `/dev/video2` is the RealSense D435I RGB camera accessed directly via OpenCV V4L2. -No pyrealsense2 SDK needed. Pure OpenCV frames (640x480 BGR). - -### Option B: RealSense SDK (pyrealsense2): -```bash -python saqr.py --source realsense --model models/saqr_best.pt --headless -python saqr.py --source realsense:243622073459 --model models/saqr_best.pt --headless -``` - -### Option C: GUI (dev machine only, not on robot): -```bash -# On your dev machine (not the robot): -python gui.py --source 0 --model models/saqr_best.pt -``` -**Note:** gui.py requires PySide6 and a display. It will NOT work on the headless Jetson robot. - -### With OpenCV camera index: -```bash -python saqr.py --source 0 --model models/saqr_best.pt --headless -``` - -### With V4L2 device path: -```bash -python saqr.py --source /dev/video0 --model models/saqr_best.pt --headless -``` - -### With GUI (if display connected): -```bash -python gui.py --source realsense --model models/saqr_best.pt -``` - -### Simple detection (no tracking): -```bash -python detect.py --source realsense --model models/saqr_best.pt -``` - ---- - -## Step 4b: Run with G1 TTS + Reject Action (Bridge) - -`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 | "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 | — | — | - -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 - -```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 -``` -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 - -```bash -xhost +local: >/dev/null 2>&1 -DISPLAY=:0 python3 saqr_g1_bridge.py --iface eth0 --source realsense -``` -`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. - -```bash -# 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 -``` - -`--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/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; 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 (use this for `--stream 8080`, `--half`, etc.) | - -### Speaker-id reference - -speaker_ids are **locked to a language** — they do NOT auto-detect input text. -On current G1 firmware, `speaker_id=0` is Chinese regardless of what you feed -it. Speaker 2 was confirmed English by running Sanad mode 6 -(`voice_example.py 6`). If the robot's firmware changes, re-scan: -```bash -# On the robot (in a conda env with unitree_sdk2py): -python3 ~/Sanad/voice_example.py 6 -``` -and pass the new id with `--speaker-id N`. - -### What a successful run looks like - -``` -[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. -[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 0002 | NEW | UNSAFE | wearing: none | missing: vest | ... -[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) - -### Live status: -```bash -cat ~/Saqr/captures/result.csv -``` - -### Event history (audit log): -```bash -cat ~/Saqr/captures/events.csv -``` - -### Captured photos: -```bash -ls ~/Saqr/captures/SAFE/ -ls ~/Saqr/captures/PARTIAL/ -ls ~/Saqr/captures/UNSAFE/ -``` - -### Export CSV report: -```bash -cd ~/Saqr -python manager.py --export -``` - -### Download results to dev machine: -```bash -# From dev machine -scp -r unitree@192.168.123.164:~/Saqr/captures/ ./captures_from_robot/ -scp unitree@192.168.123.164:~/Saqr/captures/events.csv ./events_robot.csv -``` - ---- - -## Camera Source Options - -| Source | Command | Description | -|--------|---------|-------------| -| `/dev/video2` | `--source /dev/video2` | **RGB camera via OpenCV (recommended)** | -| `realsense` | `--source realsense` | RealSense D435I via pyrealsense2 SDK | -| `realsense:SERIAL` | `--source realsense:243622073459` | Specific RealSense by serial | -| `/dev/video4` | `--source /dev/video4` | Second RGB stream (if available) | -| `0` | `--source 0` | First OpenCV camera index | -| `video.mp4` | `--source video.mp4` | Video file | -| `image.jpg` | `--source image.jpg` | Single image | - -### G1 Robot V4L2 Device Map (RealSense D435I): -``` -/dev/video0 - Stereo module (infrared) - won't open with OpenCV -/dev/video1 - Stereo metadata -/dev/video2 - RGB camera (640x480) ← USE THIS -/dev/video3 - RGB metadata -/dev/video4 - RGB camera (secondary stream) -``` - -### Detect cameras on robot: -```bash -# Find working RGB cameras -python -c " -import cv2 -for i in range(10): - cap = cv2.VideoCapture(f'/dev/video{i}', cv2.CAP_V4L2) - if cap.isOpened(): - ret, frame = cap.read() - if ret and frame is not None: - print(f'/dev/video{i}: {frame.shape} OK') - else: - print(f'/dev/video{i}: opened but no frame') - cap.release() -" - -# RealSense devices -rs-enumerate-devices | grep "Serial Number" -``` - ---- - -## Tuning Parameters - -| Parameter | Default | Flag | Description | -|-----------|---------|------|-------------| -| Confidence | 0.35 | `--conf 0.35` | Lower = more detections, higher = fewer false positives | -| Max Missing | 90 | `--max-missing 90` | Frames before track deleted (~3s at 30fps) | -| Match Distance | 250 | `--match-distance 250` | Pixels for track matching | -| Confirm Frames | 5 | `--status-confirm-frames 5` | Frames to confirm a status change | - -### Recommended for G1 patrol: -```bash -python saqr.py --source realsense --model models/saqr_best.pt --headless \ - --conf 0.30 --max-missing 120 --match-distance 300 --status-confirm-frames 7 -``` - ---- - -## Compliance Rules - -| Status | Condition | Color | -|--------|-----------|-------| -| SAFE | Helmet AND vest detected, no violations | Green | -| PARTIAL | Only helmet OR only vest detected | Yellow | -| UNSAFE | `no-helmet` or `no-vest` detected, or nothing detected | Red | - ---- - -## Output Files - -| File | Location | Description | -|------|----------|-------------| -| `result.csv` | `captures/result.csv` | Current state of all tracked persons | -| `events.csv` | `captures/events.csv` | Audit log (NEW / STATUS_CHANGE events) | -| Person crops | `captures/SAFE/*.jpg` | Cropped images of compliant workers | -| Person crops | `captures/PARTIAL/*.jpg` | Workers with incomplete PPE | -| Person crops | `captures/UNSAFE/*.jpg` | Workers violating PPE rules | -| Logs | `Logs/Inference/saqr.log` | Runtime log | - ---- - -## Project Files - -| File | Purpose | -|------|---------| -| `saqr.py` | Main PPE tracking + detection (RealSense + OpenCV) | -| `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 | -| `train.py` | YOLO model training | -| `logger.py` | Centralized logging | -| `deploy.sh` | One-command deploy to robot | -| `Config/logging.json` | Log settings | - ---- - -## Troubleshooting - -### RealSense not detected -```bash -# Check USB connection -lsusb | grep Intel - -# Re-enumerate -rs-enumerate-devices | head -10 - -# Reset USB (if needed) -sudo usbreset /dev/bus/usb/002/002 -``` - -### Camera not opening -```bash -# Test RealSense directly -python -c " -import pyrealsense2 as rs -pipe = rs.pipeline() -cfg = rs.config() -cfg.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) -pipe.start(cfg) -frames = pipe.wait_for_frames() -print('Frame:', frames.get_color_frame().get_width(), 'x', frames.get_color_frame().get_height()) -pipe.stop() -" - -# Test OpenCV fallback -python -c "import cv2; c=cv2.VideoCapture(0); print('OK' if c.isOpened() else 'FAIL'); c.release()" - -# Try different source -python saqr.py --source /dev/video0 --model models/saqr_best.pt --headless -``` - -### ModuleNotFoundError: ultralytics -```bash -# Check you're in the right conda env -which python -# Should show: /home/unitree/miniconda3/envs/teleimager/bin/python - -# Install to the correct env -python -m pip install ultralytics -``` - -### System clock wrong (SSL errors) -```bash -sudo date -s "2026-04-10 15:00:00" -``` - -### Model not found -```bash -ls ~/Saqr/models/ -# Should show: saqr_best.pt (~5.3 MB) -``` - -### Low FPS on Jetson -```bash -# Use smaller confidence to reduce load -python saqr.py --source realsense --conf 0.5 --headless - -# Or use headless opencv -export DISPLAY= -python saqr.py --source realsense --headless -``` - -### Too many duplicate track IDs -```bash -# Increase tolerance -python saqr.py --source realsense --max-missing 150 --match-distance 300 --headless -``` diff --git a/README.md b/README.md new file mode 100644 index 0000000..0ab898a --- /dev/null +++ b/README.md @@ -0,0 +1,51 @@ +# Saqr — PPE Safety Detection on Unitree G1 + +Real-time PPE compliance (helmet, vest, boots, gloves, goggles) using YOLO11n, +designed to run on a Unitree G1 humanoid with an Intel RealSense D435I. On +UNSAFE the robot speaks a warning and plays the `reject` arm action. + +## Layout + +``` +saqr/ # python package + core/ # detection + tracking + events (shared by CLI/GUI/bridge) + apps/ # CLI entry points (saqr, detect, train, manager, view_stream) + gui/ # PySide6 desktop GUI + robot/ # G1 bridge + DDS controller + utils/ # logger +scripts/ # deploy.sh, start_saqr.sh, run_local.sh, run_robot.sh, systemd unit +config/ # logging.json +data/ # dataset/, models/ (gitignored) +runtime/ # captures/, logs/, runs/ (gitignored) +docs/ # DEPLOY.md, start.md, use_case_catalogue.pdf +``` + +## Quick start + +```bash +# Install the package (editable) +pip install -e . + +# Local dev run (webcam) +saqr --source 0 + +# PySide6 GUI +pip install -e ".[gui]" +saqr-gui + +# On the Unitree G1 (bridge owns the R2+X / R2+Y flow) +saqr-bridge --iface eth0 --source realsense --headless -- --stream 8080 +``` + +Without installing, everything still works via `python -m`: + +```bash +python -m saqr.apps.saqr_cli --source 0 +python -m saqr.robot.bridge --iface eth0 --source realsense --headless +``` + +## Docs + +- [docs/DEPLOY.md](docs/DEPLOY.md) — full deploy + robot setup. +- [docs/start.md](docs/start.md) — systemd auto-start workflow. +- [docs/use_case_catalogue.pdf](docs/use_case_catalogue.pdf) — PPE use-case spec. diff --git a/Config/logging.json b/config/logging.json similarity index 100% rename from Config/logging.json rename to config/logging.json diff --git a/deploy.sh b/deploy.sh deleted file mode 100755 index 6da8361..0000000 --- a/deploy.sh +++ /dev/null @@ -1,129 +0,0 @@ -#!/bin/bash -# ============================================================================ -# Saqr PPE Detection - Deploy to Unitree G1 -# ============================================================================ -# -# Usage (from your dev machine): -# ./deploy.sh # deploy + install deps -# ./deploy.sh --run # deploy + install + start detection -# ./deploy.sh --run --source 0 # deploy + start with camera 0 -# -# ============================================================================ - -set -e - -# ── Robot config ────────────────────────────────────────────────────────────── -ROBOT_IP="${ROBOT_IP:-192.168.123.164}" -ROBOT_USER="${ROBOT_USER:-unitree}" -ROBOT_ENV="${ROBOT_ENV:-teleimager}" -REMOTE_DIR="/home/${ROBOT_USER}/Saqr" -SSH_OPTS="-o StrictHostKeyChecking=no -o ConnectTimeout=10" - -# ── Parse args ──────────────────────────────────────────────────────────────── -RUN_AFTER=false -SOURCE="0" -MODEL="models/saqr_best.pt" -HEADLESS=false - -while [[ $# -gt 0 ]]; do - case $1 in - --run) RUN_AFTER=true; shift ;; - --source) SOURCE="$2"; shift 2 ;; - --model) MODEL="$2"; shift 2 ;; - --headless) HEADLESS=true; shift ;; - --ip) ROBOT_IP="$2"; shift 2 ;; - *) echo "Unknown arg: $1"; exit 1 ;; - esac -done - -LOCAL_DIR="$(cd "$(dirname "$0")" && pwd)" - -echo "============================================" -echo " Saqr PPE - Deploy to Unitree G1" -echo "============================================" -echo " Robot : ${ROBOT_USER}@${ROBOT_IP}" -echo " Env : ${ROBOT_ENV}" -echo " Remote: ${REMOTE_DIR}" -echo "============================================" - -# ── Step 1: Test connection ─────────────────────────────────────────────────── -echo "" -echo "[1/5] Testing SSH connection..." -ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} "echo 'Connected OK'" || { - echo "[ERROR] Cannot reach ${ROBOT_IP}. Is the robot on?" - exit 1 -} - -# ── Step 2: Create remote directory ─────────────────────────────────────────── -echo "[2/5] Creating remote directory..." -ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} "mkdir -p ${REMOTE_DIR}/{models,captures/{SAFE,PARTIAL,UNSAFE},Config,Logs}" - -# ── Step 3: Copy project files ──────────────────────────────────────────────── -echo "[3/5] Copying project files..." - -# Python files -for f in saqr.py train.py detect.py manager.py logger.py gui.py requirements.txt; do - if [ -f "${LOCAL_DIR}/${f}" ]; then - scp ${SSH_OPTS} "${LOCAL_DIR}/${f}" ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/ - fi -done - -# Config -scp ${SSH_OPTS} "${LOCAL_DIR}/Config/logging.json" ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/Config/ - -# Trained model (this is the big file) -if [ -f "${LOCAL_DIR}/models/saqr_best.pt" ]; then - echo " Uploading trained model (saqr_best.pt)..." - scp ${SSH_OPTS} "${LOCAL_DIR}/models/saqr_best.pt" ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/models/ -else - echo " [WARN] models/saqr_best.pt not found - train first!" -fi - -# Base model (for retraining on robot if needed) -if [ -f "${LOCAL_DIR}/models/yolo11n.pt" ]; then - scp ${SSH_OPTS} "${LOCAL_DIR}/models/yolo11n.pt" ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/models/ -fi - -echo " Files copied." - -# ── Step 4: Install dependencies ────────────────────────────────────────────── -echo "[4/5] Installing dependencies on robot..." -ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} << 'INSTALL_EOF' - source ~/miniconda3/etc/profile.d/conda.sh - conda activate teleimager - - pip install -q ultralytics opencv-python numpy PyYAML 2>/dev/null - echo " Dependencies OK" -INSTALL_EOF - -# ── Step 5: Optionally run ──────────────────────────────────────────────────── -if [ "$RUN_AFTER" = true ]; then - echo "[5/5] Starting Saqr PPE detection on robot..." - - HEADLESS_FLAG="" - if [ "$HEADLESS" = true ]; then - HEADLESS_FLAG="--headless" - fi - - ssh ${SSH_OPTS} -t ${ROBOT_USER}@${ROBOT_IP} << RUN_EOF - source ~/miniconda3/etc/profile.d/conda.sh - conda activate teleimager - cd ${REMOTE_DIR} - python saqr.py --source ${SOURCE} --model ${MODEL} ${HEADLESS_FLAG} -RUN_EOF -else - echo "[5/5] Skipped (use --run to start after deploy)" - echo "" - echo "============================================" - echo " Deployed! SSH in to run:" - echo "============================================" - echo "" - echo " ssh ${ROBOT_USER}@${ROBOT_IP}" - echo " conda activate teleimager" - echo " cd ${REMOTE_DIR}" - echo " python saqr.py --source 0 --model models/saqr_best.pt" - echo "" - echo " Or with GUI:" - echo " python gui.py --source 0 --model models/saqr_best.pt" - echo "" -fi diff --git a/docs/DEPLOY.md b/docs/DEPLOY.md new file mode 100644 index 0000000..3b17855 --- /dev/null +++ b/docs/DEPLOY.md @@ -0,0 +1,322 @@ +# Saqr PPE Detection — Deployment Guide +## Unitree G1 Robot + Intel RealSense D435I + +--- + +## Robot Details + +| Item | Value | +|------|-------| +| Robot | Unitree G1 Humanoid | +| IP | `192.168.123.164` | +| User | `unitree` | +| OS | Ubuntu 20.04 (aarch64 / Jetson) | +| Python | 3.10 (conda env: `marcus`, legacy: `teleimager`) | +| Camera | Intel RealSense D435I | +| Serial | `243622073459` | +| Port | USB 3.2 @ `/dev/video0` | + +--- + +## Repo layout + +``` +saqr/ # python package (core/apps/gui/robot/utils) +scripts/ # deploy.sh, start_saqr.sh, run_local.sh, run_robot.sh, saqr-bridge.service +config/ # logging.json +data/ # dataset/, models/ (gitignored) +runtime/ # captures/, logs/, runs/ (gitignored) +docs/ # this file, start.md, use_case_catalogue.pdf +pyproject.toml # installs the `saqr`, `saqr-bridge`, `saqr-gui`, ... scripts +``` + +--- + +## Step 1: Train the Model (Dev Machine) + +```bash +cd ~/Robotics_workspace/yslootahtech/Project/Saqr +conda activate AI_MSI_yolo +pip install -e . +saqr-train --dataset data/dataset --epochs 100 --batch 16 +``` + +Verify model exists: +```bash +ls -lh data/models/saqr_best.pt +# Expected: ~5.3 MB +``` + +--- + +## Step 2: Deploy to Robot (Dev Machine) + +```bash +# From the project root: +scripts/deploy.sh # rsync the tree + pip install -e +scripts/deploy.sh --run # ...and start the bridge +scripts/deploy.sh --ip 10.0.0.5 # custom robot IP +``` + +The script rsyncs `saqr/`, `scripts/`, `config/`, `docs/`, +`pyproject.toml`, `requirements.txt`, and `README.md` to +`~/Saqr` on the robot, then runs `pip install -e .` inside the +target conda env (default `marcus`). + +--- + +## Step 3: Install Dependencies (Robot, one-time) + +If `scripts/deploy.sh` ran `pip install -e .` successfully, you're done. +Otherwise, on the robot: + +```bash +ssh unitree@192.168.123.164 +source ~/miniconda3/etc/profile.d/conda.sh +conda activate marcus +cd ~/Saqr +pip install -e . +``` + +### Jetson GPU PyTorch (JetPack 5.1 / CUDA 11.4) + +```bash +pip uninstall torch torchvision -y +pip install --no-cache-dir \ + https://developer.download.nvidia.com/compute/redist/jp/v51/pytorch/torch-2.1.0a0+41361538.nv23.06-cp310-cp310-linux_aarch64.whl +pip install --no-cache-dir \ + https://developer.download.nvidia.com/compute/redist/jp/v51/pytorch/torchvision-0.16.1a0+5e8e2f1-cp310-cp310-linux_aarch64.whl +``` + +### System clock (pip/SSL need a correct date) + +```bash +sudo date -s "2026-04-10 15:00:00" +``` + +### Verify + +```bash +python -c "from ultralytics import YOLO; print('ultralytics OK')" +python -c "import torch; print('CUDA:', torch.cuda.is_available())" +python -c "import cv2; print('opencv OK')" +python -c "import saqr; print('saqr', saqr.__version__)" +``` + +--- + +## Step 4: Run Saqr (Robot) + +### Production: bridge with R2+X / R2+Y + +The bridge owns the DDS clients and spawns `saqr` on demand. On the robot: + +```bash +cd ~/Saqr +scripts/start_saqr.sh # manual launch +sudo systemctl restart saqr-bridge # systemd-managed (see start.md) +``` + +Or without the helper script: + +```bash +conda activate marcus +python -m saqr.robot.bridge --iface eth0 --source realsense --headless -- --stream 8080 +``` + +### Plain saqr (no bridge) + +```bash +# With display +scripts/run_robot.sh --stream 8080 + +# Headless +scripts/run_robot.sh --headless --stream 8080 + +# V4L2 fallback if RealSense SDK won't enumerate +scripts/run_robot.sh --source /dev/video2 --headless +``` + +Equivalent `python -m` forms: + +```bash +python -m saqr.apps.saqr_cli --source realsense --model saqr_best.pt --headless +python -m saqr.apps.detect_cli --source /dev/video2 --model saqr_best.pt +python -m saqr.apps.manager_cli --export +``` + +### Dev machine GUI + +```bash +pip install -e ".[gui]" +python -m saqr.gui.app --source 0 +``` + +--- + +## Bridge behavior (R2+X / R2+Y) + +| Press | Action | +|-------|--------| +| **R2 + X** | Start saqr subprocess. Robot stays silent (no "Saqr activated." — see note below). | +| **R2 + Y** | Stop saqr, robot says **"Saqr deactivated."** Bridge stays ready. | +| **Ctrl+C** in terminal | Stop saqr (if running) and exit the bridge. | + +### Per-detection behavior (while saqr is running) + +| Transition | TTS (speaker_id=2, English) | Arm action | +|------------|-----------------------------|------------| +| → UNSAFE | "Please stop. Wear your proper safety equipment. You are missing **{items}**." | `reject` (id=13) + auto `release arm` | +| → SAFE | "Safe to enter. Have a good day." | — | +| → PARTIAL | — | — | + +Bridge startup announces **"Saqr is running. Press R2 plus X to start."**. + +> **Note on speaker_id=2:** TTS speaker_ids are locked to a language on the +> current G1 firmware. `speaker_id=0` is Chinese regardless of input text. +> `speaker_id=2` was confirmed English via `Project/Sanad/voice_example.py 6`. + +> **Note on SIGINT traceback:** when R2+Y stops saqr, Python may print a +> `KeyboardInterrupt` traceback from inside YOLO. The bridge catches the exit, +> announces "Saqr deactivated.", and stays alive. + +--- + +## Bridge CLI flags + +| Flag | Default | Description | +|------|---------|-------------| +| `--iface` | *(default DDS)* | DDS network interface, e.g. `eth0` | +| `--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 | +| `--dry-run` | off | Parse events but never call the SDK; implies `--no-trigger` | +| `--no-trigger` | off | Skip the R2+X/R2+Y loop and start saqr immediately | +| `--source` / `--headless` / `--saqr-conf` / `--imgsz` / `--device` | — | Passed through to saqr | +| `-- ` | — | Everything after `--` is forwarded raw to saqr (e.g. `-- --stream 8080`) | + +--- + +## Step 5: Check Results + +```bash +cat runtime/captures/result.csv # current state per tracked person +cat runtime/captures/events.csv # audit log +ls runtime/captures/{SAFE,PARTIAL,UNSAFE}/ + +saqr-manager --export # quick CSV export + +# Download to dev machine +scp -r unitree@192.168.123.164:~/Saqr/runtime/captures ./captures_from_robot +``` + +--- + +## Camera sources + +| Source | Flag | Notes | +|--------|------|-------| +| RealSense SDK | `--source realsense` | Uses `pyrealsense2`; preferred | +| RealSense by serial | `--source realsense:243622073459` | | +| V4L2 RGB | `--source /dev/video2` | OpenCV fallback; pure MJPG | +| First OpenCV camera | `--source 0` | | +| Video file | `--source path.mp4` | | + +G1 V4L2 map for the D435I: +``` +/dev/video0 - Stereo module (infrared) — won't open with OpenCV +/dev/video1 - Stereo metadata +/dev/video2 - RGB camera (640x480) ← USE THIS +/dev/video3 - RGB metadata +/dev/video4 - RGB camera (secondary stream) +``` + +--- + +## Tuning parameters + +| Parameter | Flag | Default | +|-----------|------|---------| +| Confidence | `--conf` | 0.35 | +| Max missing frames | `--max-missing` | 90 | +| Match distance (px) | `--match-distance` | 250 | +| Confirm frames | `--status-confirm-frames` | 5 | + +Recommended for G1 patrol: +```bash +saqr --source realsense --headless \ + --conf 0.30 --max-missing 120 --match-distance 300 --status-confirm-frames 7 +``` + +--- + +## Compliance rules + +| Status | Condition | Colour | +|--------|-----------|--------| +| SAFE | Helmet AND vest, no violations | Green | +| PARTIAL | Only helmet OR only vest | Yellow | +| UNSAFE | `no-helmet` or `no-vest`, or nothing detected | Red | + +--- + +## Output files + +| File | Location | Description | +|------|----------|-------------| +| `result.csv` | `runtime/captures/` | Current state of tracked persons | +| `events.csv` | `runtime/captures/` | Audit log (NEW / STATUS_CHANGE) | +| Person crops | `runtime/captures/{SAFE,PARTIAL,UNSAFE}/*.jpg` | Latest crop per track | +| Logs | `runtime/logs/Inference/*.log` | Module log output | + +--- + +## Source map + +| Path | Purpose | +|------|---------| +| [saqr/apps/saqr_cli.py](../saqr/apps/saqr_cli.py) | Main PPE tracking entry (`saqr`) | +| [saqr/robot/bridge.py](../saqr/robot/bridge.py) | Saqr → G1 bridge (R2+X/R2+Y) | +| [saqr/robot/robot_controller.py](../saqr/robot/robot_controller.py) | G1 arm + audio + TTS worker | +| [saqr/robot/controller.py](../saqr/robot/controller.py) | G1 wireless-remote DDS reader | +| [saqr/core/pipeline.py](../saqr/core/pipeline.py) | Per-frame detect + track + emit | +| [saqr/core/tracking.py](../saqr/core/tracking.py) | `PersonTracker`, `Track` | +| [saqr/core/events.py](../saqr/core/events.py) | Event-line format (contract with bridge) | +| [saqr/apps/detect_cli.py](../saqr/apps/detect_cli.py) | Simple detection without tracking | +| [saqr/apps/train_cli.py](../saqr/apps/train_cli.py) | YOLO11n training | +| [saqr/apps/manager_cli.py](../saqr/apps/manager_cli.py) | Capture CRUD + CSV export | +| [saqr/gui/app.py](../saqr/gui/app.py) | PySide6 desktop GUI | + +--- + +## Troubleshooting + +### RealSense not detected +```bash +lsusb | grep Intel +rs-enumerate-devices | head -10 +sudo usbreset /dev/bus/usb/002/002 # if the USB is stuck +``` + +### ModuleNotFoundError: saqr +```bash +# Make sure you're in the right env and the package is installed +which python +pip install -e ~/Saqr +``` + +### System clock wrong (SSL errors) +```bash +sudo date -s "2026-04-10 15:00:00" +``` + +### Model not found +```bash +ls ~/Saqr/data/models/ # should list saqr_best.pt +``` + +### Too many duplicate track IDs +```bash +saqr --source realsense --max-missing 150 --match-distance 300 --headless +``` diff --git a/docs/start.md b/docs/start.md new file mode 100644 index 0000000..d222874 --- /dev/null +++ b/docs/start.md @@ -0,0 +1,169 @@ +# Saqr — Auto-start on boot + +How to auto-start `saqr.robot.bridge` on every boot of the Unitree G1 +(Jetson), via `systemd` + `scripts/start_saqr.sh`. + +--- + +## Files involved + +| File | Role | +|------|------| +| `~/Saqr/saqr/robot/bridge.py` | The bridge process (DDS + TTS + R2+X/R2+Y trigger loop). Entry point: `python -m saqr.robot.bridge`. | +| `~/Saqr/scripts/start_saqr.sh` | Bash launcher: sources conda, activates `marcus`, `cd ~/Saqr`, exec the bridge with the production flags. | +| `~/Saqr/scripts/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/scripts/start_saqr.sh + +# 2. Install the systemd unit system-wide so it starts at BOOT +# (not just at login). +sudo cp ~/Saqr/scripts/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 +journalctl -u saqr-bridge -f # follow live log +sudo systemctl restart saqr-bridge +sudo systemctl stop saqr-bridge +sudo systemctl start saqr-bridge +sudo systemctl disable saqr-bridge # stop auto-starting at boot +sudo systemctl enable saqr-bridge +journalctl -u saqr-bridge -n 100 --no-pager # last 100 lines +journalctl -u saqr-bridge -b # only this boot's logs +``` + +--- + +## ⚠️ 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 `scripts/start_saqr.sh` in a terminal you'll +have two bridges fighting over the same DDS clients (`R2+X pressed -> start saqr` +followed immediately by `start ignored — saqr already running`). + +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. +sudo systemctl stop saqr-bridge +~/Saqr/scripts/start_saqr.sh +``` + +--- + +## Quick reboot test + +```bash +sudo reboot + +ssh unitree@192.168.123.164 +sudo systemctl status saqr-bridge # should be "active (running)" +journalctl -u saqr-bridge -n 50 # includes the "Saqr is running" TTS +``` + +--- + +## Updating the bridge / launcher / unit + +```bash +# If you changed start_saqr.sh or anything in saqr/: +sudo systemctl restart saqr-bridge + +# If you changed saqr-bridge.service itself: +sudo cp ~/Saqr/scripts/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` | *(parent of `scripts/`)* | Project root on the robot. | +| `CONDA_ROOT` | `$HOME/miniconda3` | Miniconda install path. | +| `CONDA_ENV` | `marcus` | Conda env with `unitree_sdk2py`, `ultralytics`, `pyrealsense2`. | +| `DDS_IFACE` | `eth0` | DDS network interface. | +| `SAQR_SOURCE` | `realsense` | `--source` passed to saqr. | +| `STREAM_PORT` | `8080` | MJPEG stream port (`-- --stream $STREAM_PORT`). | + +To override permanently in systemd, add `Environment=` lines to +`/etc/systemd/system/saqr-bridge.service`: + +```ini +Environment=SAQR_SOURCE=/dev/video2 +Environment=STREAM_PORT=9090 +``` + +Then `sudo systemctl daemon-reload && sudo systemctl restart saqr-bridge`. + +--- + +## Troubleshooting + +### Service won't start + +```bash +sudo systemctl status saqr-bridge +journalctl -u saqr-bridge -n 100 --no-pager +``` + +Common causes: +- `scripts/start_saqr.sh` not executable → `chmod +x ~/Saqr/scripts/start_saqr.sh` +- conda env name wrong → check `CONDA_ENV` +- `saqr` package not installed → `cd ~/Saqr && pip install -e .` +- `unitree_sdk2py` missing in the env → run `scripts/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 by editing the systemd unit: + +```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.robot.bridge" +sudo systemctl restart saqr-bridge +``` diff --git a/use case catalogue.pdf b/docs/use_case_catalogue.pdf similarity index 100% rename from use case catalogue.pdf rename to docs/use_case_catalogue.pdf diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..074d70b --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,31 @@ +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + +[project] +name = "saqr" +version = "0.1.0" +description = "PPE safety detection and Unitree G1 humanoid integration" +requires-python = ">=3.10" +dependencies = [ + "ultralytics>=8.0.0", + "opencv-python", + "numpy", + "PyYAML", +] + +[project.optional-dependencies] +gui = ["PySide6>=6.5.0"] +realsense = ["pyrealsense2"] + +[project.scripts] +saqr = "saqr.apps.saqr_cli:main" +saqr-detect = "saqr.apps.detect_cli:main" +saqr-train = "saqr.apps.train_cli:main" +saqr-manager = "saqr.apps.manager_cli:main" +saqr-view-stream = "saqr.apps.view_stream:main" +saqr-gui = "saqr.gui.app:main" +saqr-bridge = "saqr.robot.bridge:main" + +[tool.setuptools.packages.find] +include = ["saqr*"] diff --git a/run_local.sh b/run_local.sh deleted file mode 100755 index feb2803..0000000 --- a/run_local.sh +++ /dev/null @@ -1,110 +0,0 @@ -#!/bin/bash -# ============================================================================ -# Saqr PPE Detection - Run on Local Laptop -# ============================================================================ -# -# Usage: -# ./run_local.sh # webcam 0 -# ./run_local.sh --source 1 # webcam 1 -# ./run_local.sh --source video.mp4 # video file -# ./run_local.sh --gui # PySide6 GUI -# ./run_local.sh --detect # simple detection (no tracking) -# -# ============================================================================ - -set -e - -SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" -cd "$SCRIPT_DIR" - -# ── Defaults ────────────────────────────────────────────────────────────────── -SOURCE="0" -MODEL="models/saqr_best.pt" -CONF="0.35" -MODE="saqr" # saqr | gui | detect -HEADLESS=false -MAX_MISSING=90 -MATCH_DIST=250 -CONFIRM=5 - -# ── Parse args ──────────────────────────────────────────────────────────────── -while [[ $# -gt 0 ]]; do - case $1 in - --source) SOURCE="$2"; shift 2 ;; - --model) MODEL="$2"; shift 2 ;; - --conf) CONF="$2"; shift 2 ;; - --gui) MODE="gui"; shift ;; - --detect) MODE="detect"; shift ;; - --headless) HEADLESS=true; shift ;; - --max-missing) MAX_MISSING="$2"; shift 2 ;; - --match-distance) MATCH_DIST="$2"; shift 2 ;; - --confirm) CONFIRM="$2"; shift 2 ;; - *) echo "Unknown arg: $1"; exit 1 ;; - esac -done - -# ── Check model ─────────────────────────────────────────────────────────────── -if [ ! -f "$MODEL" ]; then - echo "[ERROR] Model not found: $MODEL" - echo "" - echo " Train first:" - echo " conda activate AI_MSI_yolo" - echo " python train.py --dataset dataset --epochs 100" - echo "" - echo " Or specify a different model:" - echo " ./run_local.sh --model /path/to/model.pt" - exit 1 -fi - -# ── Activate conda ──────────────────────────────────────────────────────────── -if command -v conda &>/dev/null; then - source "$(conda info --base)/etc/profile.d/conda.sh" 2>/dev/null || true - conda activate AI_MSI_yolo 2>/dev/null || true -fi - -echo "============================================" -echo " Saqr PPE Detection - Local Laptop" -echo "============================================" -echo " Mode : $MODE" -echo " Source : $SOURCE" -echo " Model : $MODEL" -echo " Conf : $CONF" -echo "============================================" -echo "" - -# ── Run ─────────────────────────────────────────────────────────────────────── -HEADLESS_FLAG="" -if [ "$HEADLESS" = true ]; then - HEADLESS_FLAG="--headless" -fi - -case $MODE in - saqr) - echo "Starting PPE tracking..." - echo " Press q to quit, s to save frame." - echo "" - python saqr.py \ - --source "$SOURCE" \ - --model "$MODEL" \ - --conf "$CONF" \ - --max-missing "$MAX_MISSING" \ - --match-distance "$MATCH_DIST" \ - --status-confirm-frames "$CONFIRM" \ - $HEADLESS_FLAG - ;; - gui) - echo "Starting GUI..." - python gui.py \ - --source "$SOURCE" \ - --model "$MODEL" - ;; - detect) - echo "Starting simple detection (no tracking)..." - echo " Press q to quit, s to save frame." - echo "" - python detect.py \ - --source "$SOURCE" \ - --model "$MODEL" \ - --conf "$CONF" - ;; -esac diff --git a/run_robot.sh b/run_robot.sh deleted file mode 100755 index e4741aa..0000000 --- a/run_robot.sh +++ /dev/null @@ -1,116 +0,0 @@ -#!/bin/bash -# ============================================================================ -# Saqr PPE Detection - Run on Unitree G1 Robot -# ============================================================================ -# -# Run on the robot's physical terminal (with monitor): -# ./run_robot.sh -# ./run_robot.sh --headless # no display -# ./run_robot.sh --source realsense # use pyrealsense2 SDK -# -# ============================================================================ - -set -e - -SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" -cd "$SCRIPT_DIR" - -# ── Defaults ────────────────────────────────────────────────────────────────── -SOURCE="realsense" -MODEL="models/saqr_best.pt" -CONF="0.35" -HEADLESS=false -MAX_MISSING=120 -MATCH_DIST=300 -CONFIRM=7 -DEVICE="0" -IMGSZ=320 -HALF=true -STREAM_PORT=0 - -# ── Parse args ──────────────────────────────────────────────────────────────── -while [[ $# -gt 0 ]]; do - case $1 in - --source) SOURCE="$2"; shift 2 ;; - --model) MODEL="$2"; shift 2 ;; - --conf) CONF="$2"; shift 2 ;; - --headless) HEADLESS=true; shift ;; - --max-missing) MAX_MISSING="$2"; shift 2 ;; - --match-distance) MATCH_DIST="$2"; shift 2 ;; - --confirm) CONFIRM="$2"; shift 2 ;; - --device) DEVICE="$2"; shift 2 ;; - --imgsz) IMGSZ="$2"; shift 2 ;; - --no-half) HALF=false; shift ;; - --stream) STREAM_PORT="$2"; shift 2 ;; - --cpu) DEVICE="cpu"; HALF=false; shift ;; - *) echo "Unknown arg: $1"; exit 1 ;; - esac -done - -# ── Check model ─────────────────────────────────────────────────────────────── -if [ ! -f "$MODEL" ]; then - echo "[ERROR] Model not found: $MODEL" - echo " Deploy from dev machine: ./deploy.sh" - exit 1 -fi - -# ── Activate conda ──────────────────────────────────────────────────────────── -source ~/miniconda3/etc/profile.d/conda.sh 2>/dev/null || true -conda activate teleimager 2>/dev/null || true - -# ── Fix clock if needed ────────────────────────────────────────────────────── -YEAR=$(date +%Y) -if [ "$YEAR" -lt 2025 ]; then - echo "[WARN] System clock is wrong (year=$YEAR). Fixing..." - echo "123" | sudo -S date -s "2026-04-10 16:00:00" 2>/dev/null || true -fi - -# ── Setup display ───────────────────────────────────────────────────────────── -if [ "$HEADLESS" = true ]; then - export QT_QPA_PLATFORM=offscreen - HEADLESS_FLAG="--headless" - echo "Mode: HEADLESS (no display, results saved to captures/)" -else - xhost + >/dev/null 2>&1 || true - export DISPLAY=:0 - HEADLESS_FLAG="" - echo "Mode: DISPLAY (OpenCV window on monitor)" -fi - -HALF_FLAG="" -if [ "$HALF" = true ]; then - HALF_FLAG="--half" -fi - -STREAM_FLAG="" -if [ "$STREAM_PORT" -gt 0 ]; then - STREAM_FLAG="--stream $STREAM_PORT" -fi - -echo "============================================" -echo " Saqr PPE Detection - Unitree G1 Robot" -echo "============================================" -echo " Source : $SOURCE" -echo " Model : $MODEL" -echo " Device : $DEVICE (half=$HALF, imgsz=$IMGSZ)" -echo " Conf : $CONF" -echo " Stream : ${STREAM_PORT:-disabled}" -echo " Camera : RealSense D435I" -echo "============================================" -echo "" -echo " Press q to quit, s to save frame." -echo "" - -# ── Run ─────────────────────────────────────────────────────────────────────── -python saqr.py \ - --source "$SOURCE" \ - --model "$MODEL" \ - --conf "$CONF" \ - --max-missing "$MAX_MISSING" \ - --match-distance "$MATCH_DIST" \ - --status-confirm-frames "$CONFIRM" \ - --device "$DEVICE" \ - --imgsz "$IMGSZ" \ - $HALF_FLAG \ - $STREAM_FLAG \ - $HEADLESS_FLAG diff --git a/saqr.py b/saqr.py deleted file mode 100644 index 52b0d87..0000000 --- a/saqr.py +++ /dev/null @@ -1,909 +0,0 @@ -""" -Saqr - PPE Safety Tracking -=========================== -Real-time PPE monitoring with person tracking. - -Pipeline: - 1. YOLO detection -> PPE bounding boxes (helmet, no-helmet, vest, ...) - 2. Heuristic grouping -> cluster nearby PPE boxes into person candidates - 3. Person tracker -> assign stable IDs across frames - 4. Compliance check -> SAFE / PARTIAL / UNSAFE per person - 5. Auto-capture -> save latest crop per tracked person - 6. CSV logging -> result.csv (current state) + events.csv (audit log) - -Compliance rules (helmet + vest focus): - SAFE = helmet AND vest detected, no violations - PARTIAL = only one of helmet / vest detected - UNSAFE = no-helmet or no-vest detected, or nothing detected - -Usage: - python saqr.py --source 0 # webcam (OpenCV) - python saqr.py --source realsense # Intel RealSense D435I - python saqr.py --source 1 --model models/saqr_best.pt - python saqr.py --source video.mp4 --headless -""" - -from __future__ import annotations - -import argparse -import csv -import math -import shutil -import time -from dataclasses import dataclass, field -from datetime import datetime -from pathlib import Path -from typing import Dict, List, Optional, Tuple - -import threading -from http.server import HTTPServer, BaseHTTPRequestHandler - -import cv2 -import numpy as np -from ultralytics import YOLO - -from logger import get_logger - -# Optional RealSense support -try: - import pyrealsense2 as rs - HAS_REALSENSE = True -except ImportError: - HAS_REALSENSE = False - -log = get_logger("Inference", "saqr") - -# ── Paths ───────────────────────────────────────────────────────────────────── -ROOT = Path(__file__).resolve().parent -CAPTURES_DIR = ROOT / "captures" -RESULT_CSV = CAPTURES_DIR / "result.csv" -EVENTS_CSV = CAPTURES_DIR / "events.csv" - -# ── Colours ─────────────────────────────────────────────────────────────────── -GREEN = (0, 200, 0) -YELLOW = (0, 200, 255) -RED = (0, 0, 220) -WHITE = (255, 255, 255) -BLACK = (0, 0, 0) -GRAY = (120, 120, 120) -CYAN = (200, 200, 0) - -# ── PPE class definitions ──────────────────────────────────────────────────── -STATUSES = ("SAFE", "PARTIAL", "UNSAFE") - -CLASS_ORDER = [ - "boots", "gloves", "goggles", "helmet", - "no-boots", "no-gloves", "no-goggles", "no-helmet", "no-vest", "vest", -] -PPE_SET = set(CLASS_ORDER) - -# Positive -> Negative mapping -POSITIVE_TO_NEGATIVE = { - "helmet": "no-helmet", - "vest": "no-vest", - "boots": "no-boots", - "gloves": "no-gloves", - "goggles": "no-goggles", -} -PPE_DISPLAY_ORDER = ["helmet", "vest", "gloves", "goggles", "boots"] - - -# ── Data classes ────────────────────────────────────────────────────────────── -@dataclass -class PPEItem: - label: str - conf: float - bbox: Tuple[int, int, int, int] # x1, y1, x2, y2 - - -@dataclass -class PersonCandidate: - bbox: Tuple[int, int, int, int] - items: Dict[str, float] # label -> best confidence - detections: List[PPEItem] = field(default_factory=list) - - -@dataclass -class Track: - track_id: int - bbox: Tuple[int, int, int, int] - items: Dict[str, float] - status: str - last_seen_frame: int = 0 - last_seen_iso: str = "" - created_iso: str = "" - frames_missing: int = 0 - photo_path: Optional[Path] = None - announced_status: Optional[str] = None - event_count: int = 0 - pending_status: Optional[str] = None - pending_count: int = 0 - - -# ── Utilities ───────────────────────────────────────────────────────────────── -def now_iso() -> str: - return datetime.now().isoformat(timespec="seconds") - - -def clamp_bbox(bbox, w, h): - x1, y1, x2, y2 = bbox - return max(0, x1), max(0, y1), min(w, x2), min(h, y2) - - -def expand_bbox(bbox, w, h, sx=0.8, sy=1.5): - x1, y1, x2, y2 = bbox - bw, bh = x2 - x1, y2 - y1 - cx, cy = (x1 + x2) // 2, (y1 + y2) // 2 - nw, nh = int(bw * (1 + sx)), int(bh * (1 + sy)) - nx1 = max(0, cx - nw // 2) - ny1 = max(0, cy - nh // 2) - return nx1, ny1, min(w, nx1 + nw), min(h, ny1 + nh) - - -def merge_boxes(a, b): - return (min(a[0], b[0]), min(a[1], b[1]), max(a[2], b[2]), max(a[3], b[3])) - - -def box_center(bbox): - return ((bbox[0] + bbox[2]) / 2, (bbox[1] + bbox[3]) / 2) - - -def box_distance(a, b) -> float: - ca, cb = box_center(a), box_center(b) - return math.hypot(ca[0] - cb[0], ca[1] - cb[1]) - - -def resolve_model_path(root: Path, model_arg: str) -> Path: - """Find model weights with fallback: arg -> root/arg -> models/arg.""" - p = Path(model_arg) - if p.exists(): - return p - p = root / model_arg - if p.exists(): - return p - p = root / "models" / Path(model_arg).name - if p.exists(): - return p - raise FileNotFoundError(f"Model not found: {model_arg}") - - -# ── Detection ───────────────────────────────────────────────────────────────── -# Global inference config (set by main(), read by collect_detections) -_INFER_KWARGS: Dict = {"device": "cpu", "half": False, "imgsz": 640} - - -def collect_detections(frame, model: YOLO, conf: float) -> List[PPEItem]: - """Run YOLO and return only PPE-class detections.""" - results = model(frame, conf=conf, verbose=False, **_INFER_KWARGS)[0] - items = [] - for box in results.boxes: - cls_id = int(box.cls) - label = model.names[cls_id] - if label not in PPE_SET: - continue - x1, y1, x2, y2 = map(int, box.xyxy[0]) - items.append(PPEItem(label=label, conf=float(box.conf), bbox=(x1, y1, x2, y2))) - return items - - -# ── Grouping: PPE items -> Person candidates ───────────────────────────────── -def should_merge(candidate: PersonCandidate, item: PPEItem) -> bool: - """Heuristic: is this PPE item close enough to belong to the candidate?""" - cx1, cy1, cx2, cy2 = candidate.bbox - ix1, iy1, ix2, iy2 = item.bbox - cw, ch = cx2 - cx1, cy2 - cy1 - iw, ih = ix2 - ix1, iy2 - iy1 - - cxc, cyc = (cx1 + cx2) / 2, (cy1 + cy2) / 2 - ixc, iyc = (ix1 + ix2) / 2, (iy1 + iy2) / 2 - - max_dx = max(cw, iw) * 1.2 + 40 - max_dy = max(ch, ih) * 1.8 + 50 - - return abs(ixc - cxc) <= max_dx and abs(iyc - cyc) <= max_dy - - -def group_detections_to_people(detections: List[PPEItem], w: int, h: int) -> List[PersonCandidate]: - """Cluster PPE detections into person candidates (2-pass merge).""" - if not detections: - return [] - - # Pass 1: greedy grouping - candidates: List[PersonCandidate] = [] - for item in detections: - merged = False - for cand in candidates: - if should_merge(cand, item): - cand.bbox = merge_boxes(cand.bbox, item.bbox) - cand.items[item.label] = max(cand.items.get(item.label, 0.0), item.conf) - cand.detections.append(item) - merged = True - break - if not merged: - candidates.append(PersonCandidate( - bbox=item.bbox, - items={item.label: item.conf}, - detections=[item], - )) - - # Pass 2: merge nearby candidates - again = True - while again: - again = False - merged_list: List[PersonCandidate] = [] - for person in candidates: - matched = False - for prev in merged_list: - pw = prev.bbox[2] - prev.bbox[0] - ph = prev.bbox[3] - prev.bbox[1] - dist = box_distance(prev.bbox, person.bbox) - th = max(pw, ph) * 0.55 - if dist <= th: - prev.bbox = merge_boxes(prev.bbox, person.bbox) - for label, conf in person.items.items(): - prev.items[label] = max(prev.items.get(label, 0.0), conf) - prev.detections.extend(person.detections) - again = True - matched = True - break - if not matched: - merged_list.append(person) - candidates = merged_list - - # Expand each person bbox for better crop coverage - for cand in candidates: - cand.bbox = expand_bbox(cand.bbox, w, h) - - return candidates - - -# ── Status logic (helmet + vest focus) ──────────────────────────────────────── -def status_from_items(items: Dict[str, float]) -> str: - has_helmet = items.get("helmet", 0.0) > items.get("no-helmet", 0.0) and items.get("helmet", 0.0) > 0 - has_vest = items.get("vest", 0.0) > items.get("no-vest", 0.0) and items.get("vest", 0.0) > 0 - no_helmet = items.get("no-helmet", 0.0) > 0 - no_vest = items.get("no-vest", 0.0) > 0 - - if no_helmet or no_vest: - return "UNSAFE" - if has_helmet and has_vest: - return "SAFE" - if has_helmet or has_vest: - return "PARTIAL" - return "UNSAFE" - - -def split_wearing_missing(items: Dict[str, float]) -> Tuple[List[str], List[str], List[str]]: - wearing, missing, unknown = [], [], [] - for pos in PPE_DISPLAY_ORDER: - neg = POSITIVE_TO_NEGATIVE[pos] - pos_conf = items.get(pos, 0.0) - neg_conf = items.get(neg, 0.0) - if pos_conf > neg_conf and pos_conf > 0: - wearing.append(pos) - elif neg_conf >= pos_conf and neg_conf > 0: - missing.append(pos) - else: - unknown.append(pos) - return wearing, missing, unknown - - -# ── CSV Writers ─────────────────────────────────────────────────────────────── -class EventLogger: - FIELDS = ["timestamp", "track_id", "event_type", "status", - "wearing", "missing", "unknown", "photo", "path"] - - def __init__(self, path: Path): - self.path = path - self.path.parent.mkdir(parents=True, exist_ok=True) - if not self.path.exists(): - with open(self.path, "w", newline="", encoding="utf-8") as f: - csv.DictWriter(f, fieldnames=self.FIELDS).writeheader() - - def append(self, row: Dict[str, str]) -> None: - with open(self.path, "a", newline="", encoding="utf-8") as f: - csv.DictWriter(f, fieldnames=self.FIELDS).writerow(row) - - -def write_result_csv(tracks: List[Track], output: Path) -> None: - output.parent.mkdir(parents=True, exist_ok=True) - fields = ["photo", "track_id", "status", "last_seen", - "wearing", "missing", "unknown", *CLASS_ORDER, "path"] - rows = [] - for track in sorted(tracks, key=lambda t: t.track_id): - wearing, missing, unknown = split_wearing_missing(track.items) - row = { - "photo": track.photo_path.name if track.photo_path else "", - "track_id": track.track_id, - "status": track.status, - "last_seen": track.last_seen_iso, - "wearing": ", ".join(wearing), - "missing": ", ".join(missing), - "unknown": ", ".join(unknown), - "path": str(track.photo_path) if track.photo_path else "", - } - for cls in CLASS_ORDER: - row[cls] = 1 if track.items.get(cls, 0.0) > 0 else 0 - rows.append(row) - - with open(output, "w", newline="", encoding="utf-8") as f: - w = csv.DictWriter(f, fieldnames=fields) - w.writeheader() - w.writerows(rows) - - -# ── Person Tracker ──────────────────────────────────────────────────────────── -class PersonTracker: - def __init__( - self, - event_logger: EventLogger, - max_missing: int = 90, - match_distance: float = 250.0, - status_confirm_frames: int = 5, - ): - self.event_logger = event_logger - self.max_missing = max_missing - self.match_distance = match_distance - self.status_confirm_frames = max(1, status_confirm_frames) - self.tracks: Dict[int, Track] = {} - self.next_id = 1 - - def _new_track(self, person: PersonCandidate, frame_idx: int) -> Track: - track = Track( - track_id=self.next_id, - bbox=person.bbox, - items=dict(person.items), - status=status_from_items(person.items), - last_seen_frame=frame_idx, - last_seen_iso=now_iso(), - created_iso=now_iso(), - ) - self.next_id += 1 - self.tracks[track.track_id] = track - return track - - def _match(self, person: PersonCandidate, used: set[int]) -> Optional[Track]: - best, best_dist = None, float("inf") - for tid, track in self.tracks.items(): - if tid in used: - continue - dist = box_distance(track.bbox, person.bbox) - if dist < best_dist and dist <= self.match_distance: - best_dist = dist - best = track - return best - - def update(self, people: List[PersonCandidate], frame_idx: int): - used: set[int] = set() - created: List[Track] = [] - changed: List[Track] = [] - - for person in people: - track = self._match(person, used) - if track is None: - track = self._new_track(person, frame_idx) - created.append(track) - else: - new_status = status_from_items(person.items) - track.bbox = person.bbox - track.items = dict(person.items) - track.last_seen_frame = frame_idx - track.last_seen_iso = now_iso() - track.frames_missing = 0 - - if new_status != track.status: - if track.pending_status == new_status: - track.pending_count += 1 - else: - track.pending_status = new_status - track.pending_count = 1 - if track.pending_count >= self.status_confirm_frames: - track.status = new_status - track.pending_status = None - track.pending_count = 0 - changed.append(track) - else: - track.pending_status = None - track.pending_count = 0 - - used.add(track.track_id) - - # Age and prune missing tracks - stale = [] - for tid, track in self.tracks.items(): - if tid not in used: - track.frames_missing += 1 - if track.frames_missing > self.max_missing: - stale.append(tid) - for tid in stale: - del self.tracks[tid] - - return created, changed - - def visible_tracks(self) -> List[Track]: - return [t for t in self.tracks.values() if t.frames_missing == 0] - - -# ── Track image + event ─────────────────────────────────────────────────────── -def save_track_image(frame, track: Track, capture_dirs: Dict[str, Path]) -> Optional[Path]: - h, w = frame.shape[:2] - x1, y1, x2, y2 = clamp_bbox(track.bbox, w, h) - if x2 <= x1 or y2 <= y1: - return None - crop = frame[y1:y2, x1:x2] - if crop.size == 0: - return None - - target = capture_dirs[track.status] / f"track_{track.track_id:04d}.jpg" - # Move old image if status folder changed - if track.photo_path and track.photo_path != target and track.photo_path.exists(): - try: - track.photo_path.unlink() - except OSError: - pass - - cv2.imwrite(str(target), crop) - track.photo_path = target - return target - - -def emit_event( - track: Track, - event_logger: EventLogger, - event_type: str = "STATUS_CHANGE", - force: bool = False, -) -> None: - if track.photo_path is None: - return - if not force and track.announced_status == track.status: - return - - wearing, missing, unknown = split_wearing_missing(track.items) - msg = ( - f"ID {track.track_id:04d} | {event_type} | {track.status} | " - f"wearing: {', '.join(wearing) or 'none'} | " - f"missing: {', '.join(missing) or 'none'} | " - f"unknown: {', '.join(unknown) or 'none'}" - ) - print(msg, flush=True) - - event_logger.append({ - "timestamp": now_iso(), - "track_id": str(track.track_id), - "event_type": event_type, - "status": track.status, - "wearing": ", ".join(wearing), - "missing": ", ".join(missing), - "unknown": ", ".join(unknown), - "photo": track.photo_path.name if track.photo_path else "", - "path": str(track.photo_path) if track.photo_path else "", - }) - track.announced_status = track.status - track.event_count += 1 - - -# ── Drawing ─────────────────────────────────────────────────────────────────── -def status_color(status: str) -> Tuple: - return {"SAFE": GREEN, "PARTIAL": YELLOW, "UNSAFE": RED}.get(status, GRAY) - - -def draw_track(frame, track: Track): - x1, y1, x2, y2 = track.bbox - color = status_color(track.status) - cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2) - - wearing, missing, unknown = split_wearing_missing(track.items) - line1 = f"ID {track.track_id:04d} {track.status}" - w_str = ", ".join(wearing) if wearing else "none" - m_str = ", ".join(missing) if missing else "-" - line2 = f"W:{w_str} M:{m_str}" - - (tw1, th1), _ = cv2.getTextSize(line1, cv2.FONT_HERSHEY_SIMPLEX, 0.55, 1) - (tw2, th2), _ = cv2.getTextSize(line2, cv2.FONT_HERSHEY_SIMPLEX, 0.40, 1) - tw = max(tw1, tw2) + 8 - total_h = th1 + th2 + 12 - y_top = max(0, y1 - total_h - 2) - - cv2.rectangle(frame, (x1, y_top), (x1 + tw, y1), color, -1) - cv2.putText(frame, line1, (x1 + 4, y_top + th1 + 2), - cv2.FONT_HERSHEY_SIMPLEX, 0.55, WHITE, 1, cv2.LINE_AA) - cv2.putText(frame, line2, (x1 + 4, y_top + th1 + th2 + 8), - cv2.FONT_HERSHEY_SIMPLEX, 0.40, WHITE, 1, cv2.LINE_AA) - - -def draw_counters(frame, tracks: List[Track], fps: float): - counts = {s: 0 for s in STATUSES} - for t in tracks: - counts[t.status] += 1 - - lines = [ - (f"FPS: {fps:.1f}", WHITE), - (f"SAFE {counts['SAFE']}", GREEN), - (f"PARTIAL {counts['PARTIAL']}", YELLOW), - (f"UNSAFE {counts['UNSAFE']}", RED), - (f"TRACKS {len(tracks)}", CYAN), - ] - y = 24 - for text, color in lines: - cv2.putText(frame, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, BLACK, 4, cv2.LINE_AA) - cv2.putText(frame, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2, cv2.LINE_AA) - y += 28 - - -# ── Frame processing ────────────────────────────────────────────────────────── -def process_frame( - frame, - model: YOLO, - tracker: PersonTracker, - frame_idx: int, - conf: float, - capture_dirs: Dict[str, Path], - write_csv: bool = True, -): - annotated = frame.copy() - h, w = annotated.shape[:2] - - detections = collect_detections(frame, model, conf) - candidates = group_detections_to_people(detections, w, h) - created, changed = tracker.update(candidates, frame_idx) - visible = tracker.visible_tracks() - - created_ids = {t.track_id for t in created} - changed_ids = {t.track_id for t in changed} - event_ids = created_ids | changed_ids - - for track in visible: - save_track_image(frame, track, capture_dirs) - if track.track_id in event_ids: - ev_type = "NEW" if track.track_id in created_ids else "STATUS_CHANGE" - emit_event(track, tracker.event_logger, ev_type) - draw_track(annotated, track) - - if write_csv: - write_result_csv(list(tracker.tracks.values()), RESULT_CSV) - - return annotated, visible - - -# ── MJPEG Stream Server (view on laptop browser) ───────────────────────────── -_stream_frame: Optional[bytes] = None -_stream_lock = threading.Lock() - - -class MJPEGHandler(BaseHTTPRequestHandler): - def do_GET(self): - if self.path == "/": - self.send_response(200) - self.send_header("Content-Type", "text/html") - self.end_headers() - self.wfile.write(b'' - b'' - b'') - elif self.path == "/stream": - self.send_response(200) - self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame") - self.end_headers() - while True: - with _stream_lock: - jpeg = _stream_frame - if jpeg is None: - time.sleep(0.03) - continue - try: - self.wfile.write(b"--frame\r\n" - b"Content-Type: image/jpeg\r\n\r\n" + jpeg + b"\r\n") - except BrokenPipeError: - break - else: - self.send_error(404) - - def log_message(self, format, *args): - pass # silence per-request logs - - -def start_stream_server(port: int = 8080): - server = HTTPServer(("0.0.0.0", port), MJPEGHandler) - t = threading.Thread(target=server.serve_forever, daemon=True) - t.start() - log.info(f"MJPEG stream server started on http://0.0.0.0:{port}") - return server - - -def update_stream_frame(frame): - global _stream_frame - _, jpeg = cv2.imencode(".jpg", frame, [cv2.IMWRITE_JPEG_QUALITY, 70]) - with _stream_lock: - _stream_frame = jpeg.tobytes() - - -# ── Camera / video ──────────────────────────────────────────────────────────── -class RealSenseCapture: - """Wraps pyrealsense2 pipeline with an OpenCV-like read() interface.""" - - def __init__(self, width: int = 640, height: int = 480, fps: int = 30, - serial: Optional[str] = None): - if not HAS_REALSENSE: - raise RuntimeError("pyrealsense2 not installed") - self.pipeline = rs.pipeline() - cfg = rs.config() - if serial: - cfg.enable_device(serial) - cfg.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps) - self.profile = self.pipeline.start(cfg) - self._open = True - dev = self.profile.get_device() - log.info(f"RealSense opened | {dev.get_info(rs.camera_info.name)} " - f"serial={dev.get_info(rs.camera_info.serial_number)} " - f"{width}x{height}@{fps}") - - def isOpened(self) -> bool: - return self._open - - def read(self): - if not self._open: - return False, None - try: - frames = self.pipeline.wait_for_frames(timeout_ms=3000) - color = frames.get_color_frame() - if not color: - return False, None - return True, np.asanyarray(color.get_data()) - except Exception: - return False, None - - def release(self): - if self._open: - self.pipeline.stop() - self._open = False - - -def open_capture(source: str): - # RealSense source: "realsense" or "realsense:SERIAL" - if source.lower().startswith("realsense"): - serial = None - if ":" in source: - serial = source.split(":", 1)[1] - return RealSenseCapture(width=640, height=480, fps=30, serial=serial) - - if str(source).isdigit(): - idx = int(source) - cap = cv2.VideoCapture(idx) - if cap.isOpened(): - return cap - cap = cv2.VideoCapture(idx, cv2.CAP_ANY) - if cap.isOpened(): - return cap - cap = cv2.VideoCapture(idx, cv2.CAP_V4L2) - return cap - - # V4L2 device path - if source.startswith("/dev/video"): - cap = cv2.VideoCapture(source, cv2.CAP_V4L2) - if cap.isOpened(): - cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) - cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) - cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) - cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) - cap.set(cv2.CAP_PROP_FPS, 30) - return cap - - return cv2.VideoCapture(source) - - -def setup_capture_dirs(base: Path) -> Dict[str, Path]: - dirs = {} - for s in STATUSES: - d = base / "captures" / s - d.mkdir(parents=True, exist_ok=True) - dirs[s] = d - return dirs - - -def run_video( - model: YOLO, - source: str, - conf: float, - capture_dirs: Dict[str, Path], - show_gui: bool, - csv_every_frame: bool, - max_missing: int, - match_distance: float, - status_confirm_frames: int, - stream_port: int = 0, -) -> None: - cap = open_capture(source) - if not cap.isOpened(): - log.error(f"Cannot open source: {source}") - return - - ok, first = cap.read() - if not ok or first is None or first.size == 0: - log.error(f"Cannot read first frame from source: {source}") - cap.release() - return - - event_logger = EventLogger(EVENTS_CSV) - tracker = PersonTracker( - event_logger=event_logger, - max_missing=max_missing, - match_distance=match_distance, - status_confirm_frames=status_confirm_frames, - ) - - # Start MJPEG stream server if requested - if stream_port > 0: - start_stream_server(stream_port) - - log.info(f"Session started | source={source}") - if show_gui: - print("Running - press q to quit, s to save frame.") - - prev = time.time() - frame_idx = 0 - frame = first - - while True: - frame_idx += 1 - try: - annotated, visible = process_frame( - frame, model, tracker, frame_idx, conf, - capture_dirs, write_csv=csv_every_frame, - ) - except Exception as e: - log.exception(f"Frame error #{frame_idx}: {e}") - annotated = frame - visible = tracker.visible_tracks() - - now_t = time.time() - fps = 1.0 / max(now_t - prev, 1e-9) - prev = now_t - - draw_counters(annotated, visible, fps) - - # Send to stream - if stream_port > 0: - update_stream_frame(annotated) - - if show_gui: - cv2.imshow("Saqr PPE Tracking", annotated) - key = cv2.waitKey(1) & 0xFF - if key == ord("q"): - break - if key == ord("s"): - cv2.imwrite("saved_frame.jpg", annotated) - log.info("Frame saved: saved_frame.jpg") - - ret, frame = cap.read() - if not ret: - break - - cap.release() - if show_gui: - cv2.destroyAllWindows() - - # Final CSV write - write_result_csv(list(tracker.tracks.values()), RESULT_CSV) - log.info(f"Session ended | frames={frame_idx} tracks_created={tracker.next_id - 1}") - - -def run_image(model: YOLO, path: str, conf: float, capture_dirs: Dict[str, Path], show_gui: bool): - frame = cv2.imread(path) - if frame is None: - log.error(f"Cannot read image: {path}") - return - - event_logger = EventLogger(EVENTS_CSV) - tracker = PersonTracker(event_logger=event_logger) - - annotated, visible = process_frame(frame, model, tracker, 1, conf, capture_dirs) - draw_counters(annotated, visible, 0.0) - - out = Path(path).stem + "_saqr.jpg" - cv2.imwrite(out, annotated) - log.info(f"Result saved: {out}") - - if show_gui: - cv2.imshow("Saqr PPE Tracking", annotated) - cv2.waitKey(0) - cv2.destroyAllWindows() - - -# ── CLI ─────────────────────────────────────────────────────────────────────── -def main(): - parser = argparse.ArgumentParser(description="Saqr PPE detection with tracking") - parser.add_argument("--source", default="0", - help="0/1=webcam, realsense, realsense:SERIAL, /dev/videoX, or video path") - parser.add_argument("--model", default="models/saqr_best.pt", - help="Trained YOLO weights") - parser.add_argument("--conf", type=float, default=0.35, - help="Detection confidence threshold") - parser.add_argument("--max-missing", type=int, default=90, - help="Frames to keep a lost track alive") - parser.add_argument("--match-distance", type=float, default=250.0, - help="Max pixel distance for track matching") - parser.add_argument("--status-confirm-frames", type=int, default=5, - help="Frames needed to confirm a status change") - parser.add_argument("--headless", action="store_true", - help="Disable OpenCV GUI window") - parser.add_argument("--stream", type=int, default=0, metavar="PORT", - help="Start MJPEG stream on this port (e.g. --stream 8080)") - parser.add_argument("--csv-on-exit", action="store_true", - help="Write result.csv only at session end") - # GPU / inference tuning - parser.add_argument("--device", default="0", - help="Device: 'cpu', '0' (first GPU), 'cuda:0', etc.") - parser.add_argument("--half", action="store_true", - help="Enable FP16 inference (Jetson / RTX GPUs)") - parser.add_argument("--imgsz", type=int, default=320, - help="Inference image size (320 fast, 640 accurate)") - args = parser.parse_args() - - # ── Configure global inference kwargs ──────────────────────────────── - global _INFER_KWARGS - _INFER_KWARGS = { - "device": args.device, - "half": args.half, - "imgsz": args.imgsz, - } - - # ── Log CUDA status ────────────────────────────────────────────────── - try: - import torch - if torch.cuda.is_available(): - dev_name = torch.cuda.get_device_name(0) - log.info(f"CUDA available: {dev_name} | torch={torch.__version__} | " - f"cuda={torch.version.cuda}") - else: - log.warning("CUDA not available - running on CPU (slow)") - if args.device != "cpu": - log.warning(f"Falling back to CPU (you requested device={args.device})") - _INFER_KWARGS["device"] = "cpu" - _INFER_KWARGS["half"] = False - except ImportError: - log.warning("PyTorch not found") - - log.info(f"Inference config: device={_INFER_KWARGS['device']} " - f"half={_INFER_KWARGS['half']} imgsz={_INFER_KWARGS['imgsz']}") - - capture_dirs = setup_capture_dirs(ROOT) - - try: - model_path = resolve_model_path(ROOT, args.model) - except FileNotFoundError as e: - log.error(str(e)) - log.error("Train first: python train.py --dataset dataset") - raise SystemExit(1) - - log.info(f"Loading model: {model_path}") - model = YOLO(str(model_path)) - log.info(f"Classes: {list(model.names.values())}") - - source = args.source - is_live = ( - source.isdigit() - or source.lower().startswith("realsense") - or source.startswith("/dev/video") - ) - is_video_file = source.lower().endswith( - (".mp4", ".avi", ".mov", ".mkv", ".webm") - ) - - if is_live or is_video_file: - run_video( - model, source, args.conf, capture_dirs, - show_gui=not args.headless, - csv_every_frame=not args.csv_on_exit, - max_missing=args.max_missing, - match_distance=args.match_distance, - status_confirm_frames=args.status_confirm_frames, - stream_port=args.stream, - ) - elif Path(source).exists(): - run_image(model, source, args.conf, capture_dirs, show_gui=not args.headless) - else: - log.error(f"Source not found: {source}") - raise SystemExit(1) - - -if __name__ == "__main__": - main() diff --git a/saqr/__init__.py b/saqr/__init__.py new file mode 100644 index 0000000..200f45d --- /dev/null +++ b/saqr/__init__.py @@ -0,0 +1,3 @@ +"""Saqr — PPE safety detection and G1 humanoid integration.""" + +__version__ = "0.1.0" diff --git a/saqr/apps/__init__.py b/saqr/apps/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/detect.py b/saqr/apps/detect_cli.py similarity index 70% rename from detect.py rename to saqr/apps/detect_cli.py index 7ef3cda..ffe0340 100644 --- a/detect.py +++ b/saqr/apps/detect_cli.py @@ -1,13 +1,5 @@ -""" -Saqr - PPE Detection | Simple Detection (no tracking) -======================================================== -Single-pass YOLO inference: draw PPE boxes on frame, no person tracking. -Green = PPE worn, Red = PPE missing. - -Usage: - python detect.py --source 0 - python detect.py --source image.jpg --model models/saqr_best.pt -""" +"""Simple PPE detection without person tracking.""" +from __future__ import annotations import argparse import time @@ -16,19 +8,18 @@ from pathlib import Path import cv2 from ultralytics import YOLO -from logger import get_logger +from saqr.core.detection import get_inference_config, set_inference_config +from saqr.core.model import resolve_model_path +from saqr.utils.logger import get_logger log = get_logger("Inference", "detect") -# Global inference config (set by main()) -_INFER_KWARGS: dict = {"device": "cpu", "half": False, "imgsz": 640} - VIOLATION = {"no-helmet", "no-vest", "no-boots", "no-gloves", "no-goggles"} COMPLIANT = {"helmet", "vest", "boots", "gloves", "goggles"} -GREEN = (0, 200, 0) -RED = (0, 0, 220) -BLUE = (200, 100, 0) -WHITE = (255, 255, 255) +GREEN = (0, 200, 0) +RED = (0, 0, 220) +BLUE = (200, 100, 0) +WHITE = (255, 255, 255) def box_color(label: str): @@ -42,8 +33,8 @@ def box_color(label: str): def draw_boxes(frame, results, model): for box in results.boxes: cls_id = int(box.cls) - label = model.names[cls_id] - conf = float(box.conf) + label = model.names[cls_id] + conf = float(box.conf) x1, y1, x2, y2 = map(int, box.xyxy[0]) color = box_color(label) @@ -64,11 +55,12 @@ def run_video(model, source, conf): print("Running - q to quit, s to save.") prev = time.time() + infer_kw = get_inference_config() while True: ret, frame = cap.read() if not ret: break - results = model(frame, conf=conf, verbose=False, **_INFER_KWARGS)[0] + results = model(frame, conf=conf, verbose=False, **infer_kw)[0] draw_boxes(frame, results, model) fps = 1.0 / max(time.time() - prev, 1e-9) @@ -106,30 +98,26 @@ def run_image(model, path, conf): def main(): parser = argparse.ArgumentParser(description="Saqr simple PPE detection") parser.add_argument("--source", default="0") - parser.add_argument("--model", default="models/saqr_best.pt") + parser.add_argument("--model", default="saqr_best.pt") parser.add_argument("--conf", type=float, default=0.35) parser.add_argument("--device", default="0", help="'cpu', '0', 'cuda:0'") - parser.add_argument("--half", action="store_true", help="FP16 inference") - parser.add_argument("--imgsz", type=int, default=320, help="Inference size") + parser.add_argument("--half", action="store_true") + parser.add_argument("--imgsz", type=int, default=320) args = parser.parse_args() - global _INFER_KWARGS - _INFER_KWARGS = {"device": args.device, "half": args.half, "imgsz": args.imgsz} + set_inference_config(device=args.device, half=args.half, imgsz=args.imgsz) try: import torch if not torch.cuda.is_available() and args.device != "cpu": log.warning("CUDA unavailable - falling back to CPU") - _INFER_KWARGS["device"] = "cpu" - _INFER_KWARGS["half"] = False + set_inference_config(device="cpu", half=False, imgsz=args.imgsz) except ImportError: pass - root = Path(__file__).parent - model_path = root / args.model - if not model_path.exists(): - model_path = Path(args.model) - if not model_path.exists(): - log.error(f"Model not found: {args.model}") + try: + model_path = resolve_model_path(args.model) + except FileNotFoundError as e: + log.error(str(e)) raise SystemExit(1) model = YOLO(str(model_path)) diff --git a/manager.py b/saqr/apps/manager_cli.py similarity index 81% rename from manager.py rename to saqr/apps/manager_cli.py index 6f6aabf..170168c 100644 --- a/manager.py +++ b/saqr/apps/manager_cli.py @@ -1,16 +1,4 @@ -""" -Saqr - PPE Detection | Photo Manager -======================================== -Interactive CLI to manage captured PPE photos. - -Features: list, view, move, rename, assign ID, delete, - download/copy, export CSV, update status. - -Usage: - python manager.py # interactive menu - python manager.py --export # quick CSV export -""" - +"""Interactive CLI to manage captured PPE photos + CSV export.""" from __future__ import annotations import argparse @@ -22,29 +10,27 @@ from pathlib import Path import cv2 -from logger import get_logger +from saqr.core.paths import CAPTURES_DIR, PROJECT_ROOT +from saqr.utils.logger import get_logger log = get_logger("Manager", "manager") -ROOT = Path(__file__).parent -CAPTURES_DIR = ROOT / "captures" -STATUSES = ("SAFE", "PARTIAL", "UNSAFE") -IMG_EXTS = {".jpg", ".jpeg", ".png", ".bmp", ".webp"} +STATUSES = ("SAFE", "PARTIAL", "UNSAFE") +IMG_EXTS = {".jpg", ".jpeg", ".png", ".bmp", ".webp"} CLASS_COLUMNS = [ "boots", "gloves", "goggles", "helmet", "no-boots", "no-gloves", "no-goggles", "no-helmet", "no-vest", "vest", ] -# ANSI colours _C = {"SAFE": "\033[92m", "PARTIAL": "\033[93m", "UNSAFE": "\033[91m", "BOLD": "\033[1m", "RESET": "\033[0m"} -def _cs(s): return f"{_C.get(s, '')}{s}{_C['RESET']}" + +def _cs(s): return f"{_C.get(s, '')}{s}{_C['RESET']}" def _bold(s): return f"{_C['BOLD']}{s}{_C['RESET']}" -# ── Data models ─────────────────────────────────────────────────────────────── @dataclass class Photo: path: Path @@ -55,7 +41,7 @@ class Photo: date_captured: str = "" @property - def class_flags(self) -> dict[str, int]: + def class_flags(self): flags = {c: 0 for c in CLASS_COLUMNS} stem = self.filename.lower() for c in CLASS_COLUMNS: @@ -66,7 +52,6 @@ class Photo: @dataclass class EventRow: - """One row from captures/events.csv (written by saqr.py).""" timestamp: str track_id: str event_type: str @@ -78,17 +63,16 @@ class EventRow: path: str @property - def class_flags(self) -> dict[str, int]: + def class_flags(self): worn = {c.strip() for c in self.wearing.split(",") if c.strip()} return {c: (1 if c in worn else 0) for c in CLASS_COLUMNS} @property - def missing_notes(self) -> str: + def missing_notes(self): items = [c.strip() for c in self.missing.split(",") if c.strip()] return "Missing " + ", ".join(items) if items else "Compliant" -# ── Parsing & Loading ───────────────────────────────────────────────────────── def parse_photo(path: Path, status: str) -> Photo: stem = path.stem parts = stem.split("_") @@ -96,7 +80,6 @@ def parse_photo(path: Path, status: str) -> Photo: date_captured = "" class_name = "unknown" - # Try to extract track_NNNN format if stem.startswith("track_") and len(parts) >= 2 and parts[1].isdigit(): person_id = f"track_{parts[1]}" elif len(parts) >= 2 and parts[0].isdigit() and parts[1].isdigit(): @@ -115,7 +98,7 @@ def parse_photo(path: Path, status: str) -> Photo: ) -def load_photos() -> list[Photo]: +def load_photos(): photos = [] for status in STATUSES: folder = CAPTURES_DIR / status @@ -127,7 +110,7 @@ def load_photos() -> list[Photo]: return photos -def load_events_csv(path: Path) -> list[EventRow]: +def load_events_csv(path: Path): if not path.exists(): return [] rows = [] @@ -147,7 +130,6 @@ def load_events_csv(path: Path) -> list[EventRow]: return rows -# ── Core operations ─────────────────────────────────────────────────────────── def move_photo(photo: Photo, new_status: str) -> Photo: dst_dir = CAPTURES_DIR / new_status dst_dir.mkdir(parents=True, exist_ok=True) @@ -190,7 +172,7 @@ def copy_photo(photo: Photo, dest: Path) -> Path: return dst -def export_csv(photos: list[Photo], output: Path) -> None: +def export_csv(photos, output: Path) -> None: event_rows = load_events_csv(CAPTURES_DIR / "events.csv") fields = ["photo", "track_id", "event_type", "status", "timestamp", @@ -225,7 +207,6 @@ def export_csv(photos: list[Photo], output: Path) -> None: log.info(f"CSV exported: {output} ({count} records)") -# ── Display ─────────────────────────────────────────────────────────────────── def print_header(photos): counts = {s: sum(1 for p in photos if p.status == s) for s in STATUSES} print("\n" + "=" * 66) @@ -246,7 +227,7 @@ def print_table(photos): print(f" {i+1:>4} {p.status:<8} {pid:<14} {date:<19} {p.filename[:28]}") -def pick_photo(photos, prompt="Select photo") -> Photo | None: +def pick_photo(photos, prompt="Select photo"): if not photos: print(" No photos found.") return None @@ -268,7 +249,6 @@ def show_details(photo): print(f" Path : {photo.path}") -# ── Menu actions ────────────────────────────────────────────────────────────── def act_list(photos): print("\n Filter: [1] All [2] SAFE [3] PARTIAL [4] UNSAFE") ch = input(" Choice: ").strip() @@ -299,7 +279,7 @@ def act_move(photos): if not p: return photos show_details(p) - print(f"\n Move to: [1] SAFE [2] PARTIAL [3] UNSAFE") + print("\n Move to: [1] SAFE [2] PARTIAL [3] UNSAFE") t = {"1": "SAFE", "2": "PARTIAL", "3": "UNSAFE"}.get(input(" Choice: ").strip()) if not t or t == p.status: return photos @@ -350,7 +330,7 @@ def act_download(photos): def act_export(photos): - default = ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" + default = PROJECT_ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" out = input(f" Output [{default.name}]: ").strip() output = Path(out).expanduser() if out else default export_csv(photos, output) @@ -363,7 +343,7 @@ def act_update(photos): if not p: return photos show_details(p) - print(f"\n New status: [1] SAFE [2] PARTIAL [3] UNSAFE") + print("\n New status: [1] SAFE [2] PARTIAL [3] UNSAFE") t = {"1": "SAFE", "2": "PARTIAL", "3": "UNSAFE"}.get(input(" Choice: ").strip()) if not t or t == p.status: return photos @@ -372,7 +352,6 @@ def act_update(photos): return load_photos() -# ── Main menu ───────────────────────────────────────────────────────────────── MENU = """ [1] List photos [2] View photo @@ -387,19 +366,19 @@ MENU = """ """ -def run(): +def main(): parser = argparse.ArgumentParser(description="Saqr Photo Manager") parser.add_argument("--export", action="store_true", help="Quick CSV export") args = parser.parse_args() if not CAPTURES_DIR.exists(): - print(f"[ERROR] captures/ not found. Run saqr.py first.") + print("[ERROR] runtime/captures/ not found. Run saqr first.") raise SystemExit(1) photos = load_photos() if args.export: - out = ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" + out = PROJECT_ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv" export_csv(photos, out) print(f"Exported: {out}") return @@ -434,4 +413,4 @@ def run(): if __name__ == "__main__": - run() + main() diff --git a/saqr/apps/saqr_cli.py b/saqr/apps/saqr_cli.py new file mode 100644 index 0000000..2b3307d --- /dev/null +++ b/saqr/apps/saqr_cli.py @@ -0,0 +1,193 @@ +"""Saqr PPE tracking CLI — orchestrates capture → pipeline → display/stream.""" +from __future__ import annotations + +import argparse +import time +from pathlib import Path +from typing import Dict + +import cv2 +from ultralytics import YOLO + +from saqr.core.camera import open_capture +from saqr.core.capture import setup_capture_dirs +from saqr.core.detection import set_inference_config +from saqr.core.drawing import draw_counters +from saqr.core.events import EventLogger, write_result_csv +from saqr.core.model import resolve_model_path +from saqr.core.paths import EVENTS_CSV, RESULT_CSV +from saqr.core.pipeline import process_frame +from saqr.core.streaming import start_stream_server, update_stream_frame +from saqr.core.tracking import PersonTracker +from saqr.utils.logger import get_logger + +log = get_logger("Inference", "saqr") + + +def run_video(model, source, conf, capture_dirs: Dict[str, Path], show_gui, csv_every_frame, + max_missing, match_distance, status_confirm_frames, stream_port=0): + cap = open_capture(source) + if not cap.isOpened(): + log.error(f"Cannot open source: {source}") + return + + ok, first = cap.read() + if not ok or first is None or first.size == 0: + log.error(f"Cannot read first frame from source: {source}") + cap.release() + return + + event_logger = EventLogger(EVENTS_CSV) + tracker = PersonTracker( + event_logger=event_logger, + max_missing=max_missing, + match_distance=match_distance, + status_confirm_frames=status_confirm_frames, + ) + + if stream_port > 0: + start_stream_server(stream_port) + + log.info(f"Session started | source={source}") + if show_gui: + print("Running - press q to quit, s to save frame.") + + prev = time.time() + frame_idx = 0 + frame = first + + while True: + frame_idx += 1 + try: + annotated, visible = process_frame( + frame, model, tracker, frame_idx, conf, + capture_dirs, write_csv=csv_every_frame, + ) + except Exception as e: + log.exception(f"Frame error #{frame_idx}: {e}") + annotated = frame + visible = tracker.visible_tracks() + + now_t = time.time() + fps = 1.0 / max(now_t - prev, 1e-9) + prev = now_t + + draw_counters(annotated, visible, fps) + + if stream_port > 0: + update_stream_frame(annotated) + + if show_gui: + cv2.imshow("Saqr PPE Tracking", annotated) + key = cv2.waitKey(1) & 0xFF + if key == ord("q"): + break + if key == ord("s"): + cv2.imwrite("saved_frame.jpg", annotated) + log.info("Frame saved: saved_frame.jpg") + + ret, frame = cap.read() + if not ret: + break + + cap.release() + if show_gui: + cv2.destroyAllWindows() + + write_result_csv(list(tracker.tracks.values()), RESULT_CSV) + log.info(f"Session ended | frames={frame_idx} tracks_created={tracker.next_id - 1}") + + +def run_image(model, path, conf, capture_dirs: Dict[str, Path], show_gui): + frame = cv2.imread(path) + if frame is None: + log.error(f"Cannot read image: {path}") + return + + event_logger = EventLogger(EVENTS_CSV) + tracker = PersonTracker(event_logger=event_logger) + + annotated, visible = process_frame(frame, model, tracker, 1, conf, capture_dirs) + draw_counters(annotated, visible, 0.0) + + out = Path(path).stem + "_saqr.jpg" + cv2.imwrite(out, annotated) + log.info(f"Result saved: {out}") + + if show_gui: + cv2.imshow("Saqr PPE Tracking", annotated) + cv2.waitKey(0) + cv2.destroyAllWindows() + + +def main(): + parser = argparse.ArgumentParser(description="Saqr PPE detection with tracking") + parser.add_argument("--source", default="0", + help="0/1=webcam, realsense, realsense:SERIAL, /dev/videoX, or video path") + parser.add_argument("--model", default="saqr_best.pt", + help="Trained YOLO weights (resolved under data/models/ by default)") + parser.add_argument("--conf", type=float, default=0.35) + parser.add_argument("--max-missing", type=int, default=90) + parser.add_argument("--match-distance", type=float, default=250.0) + parser.add_argument("--status-confirm-frames", type=int, default=5) + parser.add_argument("--headless", action="store_true") + parser.add_argument("--stream", type=int, default=0, metavar="PORT") + parser.add_argument("--csv-on-exit", action="store_true") + parser.add_argument("--device", default="0") + parser.add_argument("--half", action="store_true") + parser.add_argument("--imgsz", type=int, default=320) + args = parser.parse_args() + + set_inference_config(device=args.device, half=args.half, imgsz=args.imgsz) + + try: + import torch + if torch.cuda.is_available(): + log.info(f"CUDA available: {torch.cuda.get_device_name(0)} | " + f"torch={torch.__version__} | cuda={torch.version.cuda}") + else: + log.warning("CUDA not available - running on CPU (slow)") + if args.device != "cpu": + log.warning(f"Falling back to CPU (you requested device={args.device})") + set_inference_config(device="cpu", half=False, imgsz=args.imgsz) + except ImportError: + log.warning("PyTorch not found") + + capture_dirs = setup_capture_dirs() + + try: + model_path = resolve_model_path(args.model) + except FileNotFoundError as e: + log.error(str(e)) + log.error("Train first: saqr-train --dataset data/dataset") + raise SystemExit(1) + + log.info(f"Loading model: {model_path}") + model = YOLO(str(model_path)) + log.info(f"Classes: {list(model.names.values())}") + + source = args.source + is_live = (source.isdigit() + or source.lower().startswith("realsense") + or source.startswith("/dev/video")) + is_video_file = source.lower().endswith((".mp4", ".avi", ".mov", ".mkv", ".webm")) + + if is_live or is_video_file: + run_video( + model, source, args.conf, capture_dirs, + show_gui=not args.headless, + csv_every_frame=not args.csv_on_exit, + max_missing=args.max_missing, + match_distance=args.match_distance, + status_confirm_frames=args.status_confirm_frames, + stream_port=args.stream, + ) + elif Path(source).exists(): + run_image(model, source, args.conf, capture_dirs, show_gui=not args.headless) + else: + log.error(f"Source not found: {source}") + raise SystemExit(1) + + +if __name__ == "__main__": + main() diff --git a/train.py b/saqr/apps/train_cli.py similarity index 70% rename from train.py rename to saqr/apps/train_cli.py index a3b159a..55fb294 100644 --- a/train.py +++ b/saqr/apps/train_cli.py @@ -1,15 +1,5 @@ -""" -Saqr - PPE Detection | Training Script -========================================= -Train YOLO11n object detection on a PPE dataset (10 classes). - -Classes: helmet, no-helmet, vest, no-vest, boots, no-boots, - gloves, no-gloves, goggles, no-goggles - -Usage: - python train.py --dataset dataset - python train.py --dataset dataset --epochs 50 --batch 8 -""" +"""Train YOLO11n on the PPE dataset under data/dataset.""" +from __future__ import annotations import argparse import shutil @@ -17,7 +7,8 @@ from pathlib import Path import yaml -from logger import get_logger +from saqr.core.paths import DATASET_DIR, MODELS_DIR, PROJECT_ROOT, RUNS_DIR +from saqr.utils.logger import get_logger log = get_logger("Training", "train") @@ -28,7 +19,7 @@ EXPECTED_CLASSES = [ def fix_data_yaml(dataset_root: Path) -> Path: - """Ensure data.yaml has correct absolute paths for each split.""" + """Rewrite data.yaml with absolute paths for the current dataset location.""" yaml_path = dataset_root / "data.yaml" if not yaml_path.exists(): log.error(f"data.yaml not found at {yaml_path}") @@ -59,7 +50,7 @@ def fix_data_yaml(dataset_root: Path) -> Path: def main(): parser = argparse.ArgumentParser(description="Train Saqr PPE detector (YOLO11n)") - parser.add_argument("--dataset", default="dataset", + parser.add_argument("--dataset", default=str(DATASET_DIR), help="Root folder containing data.yaml + train/valid/test") parser.add_argument("--epochs", type=int, default=100) parser.add_argument("--imgsz", type=int, default=640) @@ -67,12 +58,12 @@ def main(): parser.add_argument("--model", default="yolo11n.pt", help="Base YOLO model (auto-downloaded if not present)") parser.add_argument("--name", default="saqr_det") - parser.add_argument("--device", default="0", - help="Training device: 'cpu', '0', 'cuda:0', etc.") + parser.add_argument("--device", default="0") args = parser.parse_args() - root = Path(__file__).parent - dataset_root = root / args.dataset + dataset_root = Path(args.dataset) + if not dataset_root.is_absolute(): + dataset_root = PROJECT_ROOT / dataset_root if not dataset_root.exists(): log.error(f"Dataset folder not found: {dataset_root}") raise SystemExit(1) @@ -81,8 +72,13 @@ def main(): from ultralytics import YOLO - log.info(f"Loading base model: {args.model}") - model = YOLO(args.model) + base = Path(args.model) + if not base.is_absolute() and not base.exists(): + candidate = MODELS_DIR / base.name + if candidate.exists(): + base = candidate + log.info(f"Loading base model: {base}") + model = YOLO(str(base)) log.info(f"Training | epochs={args.epochs} imgsz={args.imgsz} " f"batch={args.batch} device={args.device}") @@ -93,25 +89,22 @@ def main(): batch=args.batch, device=args.device, name=args.name, - project=str(root / "runs" / "train"), + project=str(RUNS_DIR / "train"), exist_ok=True, ) - # Copy best/last weights to models/ - models_dir = root / "models" - models_dir.mkdir(exist_ok=True) - weights_dir = root / "runs" / "train" / args.name / "weights" - + MODELS_DIR.mkdir(parents=True, exist_ok=True) + weights_dir = RUNS_DIR / "train" / args.name / "weights" for name in ("best.pt", "last.pt"): src = weights_dir / name - dst = models_dir / f"saqr_{name}" + dst = MODELS_DIR / f"saqr_{name}" if src.exists(): shutil.copy(src, dst) log.info(f"Saved: {dst}") metrics = model.val() log.info(f"mAP50={metrics.box.map50:.4f} mAP50-95={metrics.box.map:.4f}") - log.info("Next: python saqr.py --source 0") + log.info("Next: saqr --source 0") if __name__ == "__main__": diff --git a/view_stream.py b/saqr/apps/view_stream.py similarity index 66% rename from view_stream.py rename to saqr/apps/view_stream.py index 967b705..2a31d35 100644 --- a/view_stream.py +++ b/saqr/apps/view_stream.py @@ -1,21 +1,15 @@ -""" -Saqr - View robot PPE stream on laptop via OpenCV -=================================================== -Connects to the robot's MJPEG stream and displays in an OpenCV window. - -Usage: - python view_stream.py - python view_stream.py --ip 192.168.123.164 - python view_stream.py --ip 10.255.254.86 --port 8080 -""" +"""View the robot's MJPEG stream on a laptop.""" +from __future__ import annotations import argparse + import cv2 + def main(): parser = argparse.ArgumentParser(description="View Saqr PPE stream from robot") parser.add_argument("--ip", default="192.168.123.164", help="Robot IP address") - parser.add_argument("--port", default="8080", help="Stream port") + parser.add_argument("--port", default="8080", help="Stream port") args = parser.parse_args() url = f"http://{args.ip}:{args.port}/stream" @@ -24,7 +18,6 @@ def main(): cap = cv2.VideoCapture(url) if not cap.isOpened(): print(f"[ERROR] Cannot connect to {url}") - print(f" Try: python view_stream.py --ip 10.255.254.86") return print("Connected! Press q to quit.") @@ -44,5 +37,6 @@ def main(): cap.release() cv2.destroyAllWindows() + if __name__ == "__main__": main() diff --git a/saqr/core/__init__.py b/saqr/core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/saqr/core/camera.py b/saqr/core/camera.py new file mode 100644 index 0000000..5ed53bd --- /dev/null +++ b/saqr/core/camera.py @@ -0,0 +1,88 @@ +"""Camera backends: RealSense SDK and OpenCV V4L2.""" +from __future__ import annotations + +from typing import Optional + +import cv2 +import numpy as np + +from saqr.utils.logger import get_logger + +log = get_logger("Inference", "camera") + +try: + import pyrealsense2 as rs + HAS_REALSENSE = True +except ImportError: + HAS_REALSENSE = False + + +class RealSenseCapture: + """pyrealsense2 pipeline with an OpenCV-like read() interface.""" + + def __init__(self, width: int = 640, height: int = 480, fps: int = 30, + serial: Optional[str] = None): + if not HAS_REALSENSE: + raise RuntimeError("pyrealsense2 not installed") + self.pipeline = rs.pipeline() + cfg = rs.config() + if serial: + cfg.enable_device(serial) + cfg.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps) + self.profile = self.pipeline.start(cfg) + self._open = True + dev = self.profile.get_device() + log.info(f"RealSense opened | {dev.get_info(rs.camera_info.name)} " + f"serial={dev.get_info(rs.camera_info.serial_number)} " + f"{width}x{height}@{fps}") + + def isOpened(self) -> bool: + return self._open + + def read(self): + if not self._open: + return False, None + try: + frames = self.pipeline.wait_for_frames(timeout_ms=3000) + color = frames.get_color_frame() + if not color: + return False, None + return True, np.asanyarray(color.get_data()) + except Exception: + return False, None + + def release(self): + if self._open: + self.pipeline.stop() + self._open = False + + +def open_capture(source: str): + if source.lower().startswith("realsense"): + serial = None + if ":" in source: + serial = source.split(":", 1)[1] + return RealSenseCapture(width=640, height=480, fps=30, serial=serial) + + if str(source).isdigit(): + idx = int(source) + cap = cv2.VideoCapture(idx) + if cap.isOpened(): + return cap + cap = cv2.VideoCapture(idx, cv2.CAP_ANY) + if cap.isOpened(): + return cap + cap = cv2.VideoCapture(idx, cv2.CAP_V4L2) + return cap + + if source.startswith("/dev/video"): + cap = cv2.VideoCapture(source, cv2.CAP_V4L2) + if cap.isOpened(): + cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) + cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) + cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) + cap.set(cv2.CAP_PROP_FPS, 30) + return cap + + return cv2.VideoCapture(source) diff --git a/saqr/core/capture.py b/saqr/core/capture.py new file mode 100644 index 0000000..f3c6a64 --- /dev/null +++ b/saqr/core/capture.py @@ -0,0 +1,41 @@ +"""Per-track image cropping + capture directory setup.""" +from __future__ import annotations + +from pathlib import Path +from typing import Dict, Optional + +import cv2 + +from saqr.core.detection import STATUSES +from saqr.core.geometry import clamp_bbox +from saqr.core.paths import CAPTURES_DIR + + +def setup_capture_dirs() -> Dict[str, Path]: + dirs: Dict[str, Path] = {} + for s in STATUSES: + d = CAPTURES_DIR / s + d.mkdir(parents=True, exist_ok=True) + dirs[s] = d + return dirs + + +def save_track_image(frame, track, capture_dirs: Dict[str, Path]) -> Optional[Path]: + h, w = frame.shape[:2] + x1, y1, x2, y2 = clamp_bbox(track.bbox, w, h) + if x2 <= x1 or y2 <= y1: + return None + crop = frame[y1:y2, x1:x2] + if crop.size == 0: + return None + + target = capture_dirs[track.status] / f"track_{track.track_id:04d}.jpg" + if track.photo_path and track.photo_path != target and track.photo_path.exists(): + try: + track.photo_path.unlink() + except OSError: + pass + + cv2.imwrite(str(target), crop) + track.photo_path = target + return target diff --git a/saqr/core/compliance.py b/saqr/core/compliance.py new file mode 100644 index 0000000..6d82560 --- /dev/null +++ b/saqr/core/compliance.py @@ -0,0 +1,36 @@ +"""SAFE / PARTIAL / UNSAFE classification (helmet + vest focus).""" +from __future__ import annotations + +from typing import Dict, List, Tuple + +from saqr.core.detection import POSITIVE_TO_NEGATIVE, PPE_DISPLAY_ORDER + + +def status_from_items(items: Dict[str, float]) -> str: + has_helmet = items.get("helmet", 0.0) > items.get("no-helmet", 0.0) and items.get("helmet", 0.0) > 0 + has_vest = items.get("vest", 0.0) > items.get("no-vest", 0.0) and items.get("vest", 0.0) > 0 + no_helmet = items.get("no-helmet", 0.0) > 0 + no_vest = items.get("no-vest", 0.0) > 0 + + if no_helmet or no_vest: + return "UNSAFE" + if has_helmet and has_vest: + return "SAFE" + if has_helmet or has_vest: + return "PARTIAL" + return "UNSAFE" + + +def split_wearing_missing(items: Dict[str, float]) -> Tuple[List[str], List[str], List[str]]: + wearing, missing, unknown = [], [], [] + for pos in PPE_DISPLAY_ORDER: + neg = POSITIVE_TO_NEGATIVE[pos] + pos_conf = items.get(pos, 0.0) + neg_conf = items.get(neg, 0.0) + if pos_conf > neg_conf and pos_conf > 0: + wearing.append(pos) + elif neg_conf >= pos_conf and neg_conf > 0: + missing.append(pos) + else: + unknown.append(pos) + return wearing, missing, unknown diff --git a/saqr/core/detection.py b/saqr/core/detection.py new file mode 100644 index 0000000..445136d --- /dev/null +++ b/saqr/core/detection.py @@ -0,0 +1,56 @@ +"""YOLO inference and PPE class tables.""" +from __future__ import annotations + +from dataclasses import dataclass +from typing import Dict, List, Tuple + +from ultralytics import YOLO + +STATUSES = ("SAFE", "PARTIAL", "UNSAFE") + +CLASS_ORDER = [ + "boots", "gloves", "goggles", "helmet", + "no-boots", "no-gloves", "no-goggles", "no-helmet", "no-vest", "vest", +] +PPE_SET = set(CLASS_ORDER) + +POSITIVE_TO_NEGATIVE = { + "helmet": "no-helmet", + "vest": "no-vest", + "boots": "no-boots", + "gloves": "no-gloves", + "goggles": "no-goggles", +} +PPE_DISPLAY_ORDER = ["helmet", "vest", "gloves", "goggles", "boots"] + + +@dataclass +class PPEItem: + label: str + conf: float + bbox: Tuple[int, int, int, int] + + +_INFER_KWARGS: Dict = {"device": "cpu", "half": False, "imgsz": 640} + + +def set_inference_config(*, device: str, half: bool, imgsz: int) -> None: + _INFER_KWARGS.update(device=device, half=half, imgsz=imgsz) + + +def get_inference_config() -> Dict: + return dict(_INFER_KWARGS) + + +def collect_detections(frame, model: YOLO, conf: float) -> List[PPEItem]: + """Run YOLO and return only PPE-class detections.""" + results = model(frame, conf=conf, verbose=False, **_INFER_KWARGS)[0] + items: List[PPEItem] = [] + for box in results.boxes: + cls_id = int(box.cls) + label = model.names[cls_id] + if label not in PPE_SET: + continue + x1, y1, x2, y2 = map(int, box.xyxy[0]) + items.append(PPEItem(label=label, conf=float(box.conf), bbox=(x1, y1, x2, y2))) + return items diff --git a/saqr/core/drawing.py b/saqr/core/drawing.py new file mode 100644 index 0000000..e09763a --- /dev/null +++ b/saqr/core/drawing.py @@ -0,0 +1,64 @@ +"""OpenCV overlays for tracked people and on-screen counters.""" +from __future__ import annotations + +from typing import List, Tuple + +import cv2 + +from saqr.core.compliance import split_wearing_missing +from saqr.core.detection import STATUSES + +GREEN = (0, 200, 0) +YELLOW = (0, 200, 255) +RED = (0, 0, 220) +WHITE = (255, 255, 255) +BLACK = (0, 0, 0) +GRAY = (120, 120, 120) +CYAN = (200, 200, 0) + + +def status_color(status: str) -> Tuple: + return {"SAFE": GREEN, "PARTIAL": YELLOW, "UNSAFE": RED}.get(status, GRAY) + + +def draw_track(frame, track): + x1, y1, x2, y2 = track.bbox + color = status_color(track.status) + cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2) + + wearing, missing, _unknown = split_wearing_missing(track.items) + line1 = f"ID {track.track_id:04d} {track.status}" + w_str = ", ".join(wearing) if wearing else "none" + m_str = ", ".join(missing) if missing else "-" + line2 = f"W:{w_str} M:{m_str}" + + (tw1, th1), _ = cv2.getTextSize(line1, cv2.FONT_HERSHEY_SIMPLEX, 0.55, 1) + (tw2, th2), _ = cv2.getTextSize(line2, cv2.FONT_HERSHEY_SIMPLEX, 0.40, 1) + tw = max(tw1, tw2) + 8 + total_h = th1 + th2 + 12 + y_top = max(0, y1 - total_h - 2) + + cv2.rectangle(frame, (x1, y_top), (x1 + tw, y1), color, -1) + cv2.putText(frame, line1, (x1 + 4, y_top + th1 + 2), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, WHITE, 1, cv2.LINE_AA) + cv2.putText(frame, line2, (x1 + 4, y_top + th1 + th2 + 8), + cv2.FONT_HERSHEY_SIMPLEX, 0.40, WHITE, 1, cv2.LINE_AA) + + +def draw_counters(frame, tracks: List, fps: float): + counts = {s: 0 for s in STATUSES} + for t in tracks: + counts[t.status] += 1 + + lines = [ + (f"FPS: {fps:.1f}", WHITE), + (f"SAFE {counts['SAFE']}", GREEN), + (f"PARTIAL {counts['PARTIAL']}", YELLOW), + (f"UNSAFE {counts['UNSAFE']}", RED), + (f"TRACKS {len(tracks)}", CYAN), + ] + y = 24 + for text, color in lines: + cv2.putText(frame, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, BLACK, 4, cv2.LINE_AA) + cv2.putText(frame, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2, cv2.LINE_AA) + y += 28 diff --git a/saqr/core/events.py b/saqr/core/events.py new file mode 100644 index 0000000..04d433e --- /dev/null +++ b/saqr/core/events.py @@ -0,0 +1,93 @@ +"""Event-line emission and CSV writers. + +The ``emit_event`` output line is a contract with ``saqr.robot.bridge`` — its +regex parses this exact format. Don't change the field order without updating +the bridge. +""" +from __future__ import annotations + +import csv +from datetime import datetime +from pathlib import Path +from typing import Dict, List + +from saqr.core.compliance import split_wearing_missing +from saqr.core.detection import CLASS_ORDER + + +def now_iso() -> str: + return datetime.now().isoformat(timespec="seconds") + + +class EventLogger: + FIELDS = ["timestamp", "track_id", "event_type", "status", + "wearing", "missing", "unknown", "photo", "path"] + + def __init__(self, path: Path): + self.path = path + self.path.parent.mkdir(parents=True, exist_ok=True) + if not self.path.exists(): + with open(self.path, "w", newline="", encoding="utf-8") as f: + csv.DictWriter(f, fieldnames=self.FIELDS).writeheader() + + def append(self, row: Dict[str, str]) -> None: + with open(self.path, "a", newline="", encoding="utf-8") as f: + csv.DictWriter(f, fieldnames=self.FIELDS).writerow(row) + + +def write_result_csv(tracks: List, output: Path) -> None: + output.parent.mkdir(parents=True, exist_ok=True) + fields = ["photo", "track_id", "status", "last_seen", + "wearing", "missing", "unknown", *CLASS_ORDER, "path"] + rows = [] + for track in sorted(tracks, key=lambda t: t.track_id): + wearing, missing, unknown = split_wearing_missing(track.items) + row = { + "photo": track.photo_path.name if track.photo_path else "", + "track_id": track.track_id, + "status": track.status, + "last_seen": track.last_seen_iso, + "wearing": ", ".join(wearing), + "missing": ", ".join(missing), + "unknown": ", ".join(unknown), + "path": str(track.photo_path) if track.photo_path else "", + } + for cls in CLASS_ORDER: + row[cls] = 1 if track.items.get(cls, 0.0) > 0 else 0 + rows.append(row) + + with open(output, "w", newline="", encoding="utf-8") as f: + w = csv.DictWriter(f, fieldnames=fields) + w.writeheader() + w.writerows(rows) + + +def emit_event(track, event_logger: EventLogger, event_type: str = "STATUS_CHANGE", + force: bool = False) -> None: + if track.photo_path is None: + return + if not force and track.announced_status == track.status: + return + + wearing, missing, unknown = split_wearing_missing(track.items) + msg = ( + f"ID {track.track_id:04d} | {event_type} | {track.status} | " + f"wearing: {', '.join(wearing) or 'none'} | " + f"missing: {', '.join(missing) or 'none'} | " + f"unknown: {', '.join(unknown) or 'none'}" + ) + print(msg, flush=True) + + event_logger.append({ + "timestamp": now_iso(), + "track_id": str(track.track_id), + "event_type": event_type, + "status": track.status, + "wearing": ", ".join(wearing), + "missing": ", ".join(missing), + "unknown": ", ".join(unknown), + "photo": track.photo_path.name if track.photo_path else "", + "path": str(track.photo_path) if track.photo_path else "", + }) + track.announced_status = track.status + track.event_count += 1 diff --git a/saqr/core/geometry.py b/saqr/core/geometry.py new file mode 100644 index 0000000..9d7c687 --- /dev/null +++ b/saqr/core/geometry.py @@ -0,0 +1,35 @@ +"""Bounding-box math shared by grouping, tracking, capture, drawing.""" +from __future__ import annotations + +import math +from typing import Tuple + +Bbox = Tuple[int, int, int, int] + + +def clamp_bbox(bbox: Bbox, w: int, h: int) -> Bbox: + x1, y1, x2, y2 = bbox + return max(0, x1), max(0, y1), min(w, x2), min(h, y2) + + +def expand_bbox(bbox: Bbox, w: int, h: int, sx: float = 0.8, sy: float = 1.5) -> Bbox: + x1, y1, x2, y2 = bbox + bw, bh = x2 - x1, y2 - y1 + cx, cy = (x1 + x2) // 2, (y1 + y2) // 2 + nw, nh = int(bw * (1 + sx)), int(bh * (1 + sy)) + nx1 = max(0, cx - nw // 2) + ny1 = max(0, cy - nh // 2) + return nx1, ny1, min(w, nx1 + nw), min(h, ny1 + nh) + + +def merge_boxes(a: Bbox, b: Bbox) -> Bbox: + return (min(a[0], b[0]), min(a[1], b[1]), max(a[2], b[2]), max(a[3], b[3])) + + +def box_center(bbox: Bbox): + return ((bbox[0] + bbox[2]) / 2, (bbox[1] + bbox[3]) / 2) + + +def box_distance(a: Bbox, b: Bbox) -> float: + ca, cb = box_center(a), box_center(b) + return math.hypot(ca[0] - cb[0], ca[1] - cb[1]) diff --git a/saqr/core/grouping.py b/saqr/core/grouping.py new file mode 100644 index 0000000..9360779 --- /dev/null +++ b/saqr/core/grouping.py @@ -0,0 +1,80 @@ +"""Cluster PPE detections into per-person candidates.""" +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Dict, List, Tuple + +from saqr.core.detection import PPEItem +from saqr.core.geometry import box_distance, expand_bbox, merge_boxes + + +@dataclass +class PersonCandidate: + bbox: Tuple[int, int, int, int] + items: Dict[str, float] + detections: List[PPEItem] = field(default_factory=list) + + +def should_merge(candidate: PersonCandidate, item: PPEItem) -> bool: + cx1, cy1, cx2, cy2 = candidate.bbox + ix1, iy1, ix2, iy2 = item.bbox + cw, ch = cx2 - cx1, cy2 - cy1 + iw, ih = ix2 - ix1, iy2 - iy1 + + cxc, cyc = (cx1 + cx2) / 2, (cy1 + cy2) / 2 + ixc, iyc = (ix1 + ix2) / 2, (iy1 + iy2) / 2 + + max_dx = max(cw, iw) * 1.2 + 40 + max_dy = max(ch, ih) * 1.8 + 50 + + return abs(ixc - cxc) <= max_dx and abs(iyc - cyc) <= max_dy + + +def group_detections_to_people(detections: List[PPEItem], w: int, h: int) -> List[PersonCandidate]: + if not detections: + return [] + + candidates: List[PersonCandidate] = [] + for item in detections: + merged = False + for cand in candidates: + if should_merge(cand, item): + cand.bbox = merge_boxes(cand.bbox, item.bbox) + cand.items[item.label] = max(cand.items.get(item.label, 0.0), item.conf) + cand.detections.append(item) + merged = True + break + if not merged: + candidates.append(PersonCandidate( + bbox=item.bbox, + items={item.label: item.conf}, + detections=[item], + )) + + again = True + while again: + again = False + merged_list: List[PersonCandidate] = [] + for person in candidates: + matched = False + for prev in merged_list: + pw = prev.bbox[2] - prev.bbox[0] + ph = prev.bbox[3] - prev.bbox[1] + dist = box_distance(prev.bbox, person.bbox) + th = max(pw, ph) * 0.55 + if dist <= th: + prev.bbox = merge_boxes(prev.bbox, person.bbox) + for label, conf in person.items.items(): + prev.items[label] = max(prev.items.get(label, 0.0), conf) + prev.detections.extend(person.detections) + again = True + matched = True + break + if not matched: + merged_list.append(person) + candidates = merged_list + + for cand in candidates: + cand.bbox = expand_bbox(cand.bbox, w, h) + + return candidates diff --git a/saqr/core/model.py b/saqr/core/model.py new file mode 100644 index 0000000..b24d3b3 --- /dev/null +++ b/saqr/core/model.py @@ -0,0 +1,20 @@ +"""Model-weight discovery across the new data/models layout.""" +from __future__ import annotations + +from pathlib import Path + +from saqr.core.paths import MODELS_DIR, PROJECT_ROOT + + +def resolve_model_path(model_arg: str) -> Path: + """Find weights: arg -> PROJECT_ROOT/arg -> data/models/.""" + p = Path(model_arg) + if p.exists(): + return p + p = PROJECT_ROOT / model_arg + if p.exists(): + return p + p = MODELS_DIR / Path(model_arg).name + if p.exists(): + return p + raise FileNotFoundError(f"Model not found: {model_arg}") diff --git a/saqr/core/paths.py b/saqr/core/paths.py new file mode 100644 index 0000000..7cb3a4f --- /dev/null +++ b/saqr/core/paths.py @@ -0,0 +1,19 @@ +"""Canonical project paths, resolved from the saqr package location.""" +from __future__ import annotations + +from pathlib import Path + +PROJECT_ROOT = Path(__file__).resolve().parents[2] + +CONFIG_DIR = PROJECT_ROOT / "config" +DATA_DIR = PROJECT_ROOT / "data" +DATASET_DIR = DATA_DIR / "dataset" +MODELS_DIR = DATA_DIR / "models" + +RUNTIME_DIR = PROJECT_ROOT / "runtime" +CAPTURES_DIR = RUNTIME_DIR / "captures" +LOGS_DIR = RUNTIME_DIR / "logs" +RUNS_DIR = RUNTIME_DIR / "runs" + +RESULT_CSV = CAPTURES_DIR / "result.csv" +EVENTS_CSV = CAPTURES_DIR / "events.csv" diff --git a/saqr/core/pipeline.py b/saqr/core/pipeline.py new file mode 100644 index 0000000..bba3e81 --- /dev/null +++ b/saqr/core/pipeline.py @@ -0,0 +1,40 @@ +"""Per-frame detect + group + track + capture + emit pipeline.""" +from __future__ import annotations + +from pathlib import Path +from typing import Dict + +from saqr.core.capture import save_track_image +from saqr.core.detection import collect_detections +from saqr.core.drawing import draw_track +from saqr.core.events import emit_event, write_result_csv +from saqr.core.grouping import group_detections_to_people +from saqr.core.paths import RESULT_CSV +from saqr.core.tracking import PersonTracker + + +def process_frame(frame, model, tracker: PersonTracker, frame_idx: int, conf: float, + capture_dirs: Dict[str, Path], write_csv: bool = True): + annotated = frame.copy() + h, w = annotated.shape[:2] + + detections = collect_detections(frame, model, conf) + candidates = group_detections_to_people(detections, w, h) + created, changed = tracker.update(candidates, frame_idx) + visible = tracker.visible_tracks() + + created_ids = {t.track_id for t in created} + changed_ids = {t.track_id for t in changed} + event_ids = created_ids | changed_ids + + for track in visible: + save_track_image(frame, track, capture_dirs) + if track.track_id in event_ids: + ev_type = "NEW" if track.track_id in created_ids else "STATUS_CHANGE" + emit_event(track, tracker.event_logger, ev_type) + draw_track(annotated, track) + + if write_csv: + write_result_csv(list(tracker.tracks.values()), RESULT_CSV) + + return annotated, visible diff --git a/saqr/core/streaming.py b/saqr/core/streaming.py new file mode 100644 index 0000000..a81ec78 --- /dev/null +++ b/saqr/core/streaming.py @@ -0,0 +1,62 @@ +"""MJPEG HTTP stream server for browser-based monitoring.""" +from __future__ import annotations + +import threading +import time +from http.server import BaseHTTPRequestHandler, HTTPServer +from typing import Optional + +import cv2 + +from saqr.utils.logger import get_logger + +log = get_logger("Inference", "streaming") + +_stream_frame: Optional[bytes] = None +_stream_lock = threading.Lock() + + +class MJPEGHandler(BaseHTTPRequestHandler): + def do_GET(self): + if self.path == "/": + self.send_response(200) + self.send_header("Content-Type", "text/html") + self.end_headers() + self.wfile.write(b'' + b'' + b'') + elif self.path == "/stream": + self.send_response(200) + self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame") + self.end_headers() + while True: + with _stream_lock: + jpeg = _stream_frame + if jpeg is None: + time.sleep(0.03) + continue + try: + self.wfile.write(b"--frame\r\n" + b"Content-Type: image/jpeg\r\n\r\n" + jpeg + b"\r\n") + except BrokenPipeError: + break + else: + self.send_error(404) + + def log_message(self, format, *args): + pass + + +def start_stream_server(port: int = 8080): + server = HTTPServer(("0.0.0.0", port), MJPEGHandler) + t = threading.Thread(target=server.serve_forever, daemon=True) + t.start() + log.info(f"MJPEG stream server started on http://0.0.0.0:{port}") + return server + + +def update_stream_frame(frame): + global _stream_frame + _, jpeg = cv2.imencode(".jpg", frame, [cv2.IMWRITE_JPEG_QUALITY, 70]) + with _stream_lock: + _stream_frame = jpeg.tobytes() diff --git a/saqr/core/tracking.py b/saqr/core/tracking.py new file mode 100644 index 0000000..59bb1f1 --- /dev/null +++ b/saqr/core/tracking.py @@ -0,0 +1,118 @@ +"""Per-person Track dataclass and the greedy nearest-match PersonTracker.""" +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Dict, List, Optional, Tuple + +from saqr.core.compliance import status_from_items +from saqr.core.events import EventLogger, now_iso +from saqr.core.geometry import box_distance +from saqr.core.grouping import PersonCandidate + + +@dataclass +class Track: + track_id: int + bbox: Tuple[int, int, int, int] + items: Dict[str, float] + status: str + last_seen_frame: int = 0 + last_seen_iso: str = "" + created_iso: str = "" + frames_missing: int = 0 + photo_path: Optional[Path] = None + announced_status: Optional[str] = None + event_count: int = 0 + pending_status: Optional[str] = None + pending_count: int = 0 + + +class PersonTracker: + def __init__( + self, + event_logger: EventLogger, + max_missing: int = 90, + match_distance: float = 250.0, + status_confirm_frames: int = 5, + ): + self.event_logger = event_logger + self.max_missing = max_missing + self.match_distance = match_distance + self.status_confirm_frames = max(1, status_confirm_frames) + self.tracks: Dict[int, Track] = {} + self.next_id = 1 + + def _new_track(self, person: PersonCandidate, frame_idx: int) -> Track: + track = Track( + track_id=self.next_id, + bbox=person.bbox, + items=dict(person.items), + status=status_from_items(person.items), + last_seen_frame=frame_idx, + last_seen_iso=now_iso(), + created_iso=now_iso(), + ) + self.next_id += 1 + self.tracks[track.track_id] = track + return track + + def _match(self, person: PersonCandidate, used: set) -> Optional[Track]: + best, best_dist = None, float("inf") + for tid, track in self.tracks.items(): + if tid in used: + continue + dist = box_distance(track.bbox, person.bbox) + if dist < best_dist and dist <= self.match_distance: + best_dist = dist + best = track + return best + + def update(self, people: List[PersonCandidate], frame_idx: int): + used: set = set() + created: List[Track] = [] + changed: List[Track] = [] + + for person in people: + track = self._match(person, used) + if track is None: + track = self._new_track(person, frame_idx) + created.append(track) + else: + new_status = status_from_items(person.items) + track.bbox = person.bbox + track.items = dict(person.items) + track.last_seen_frame = frame_idx + track.last_seen_iso = now_iso() + track.frames_missing = 0 + + if new_status != track.status: + if track.pending_status == new_status: + track.pending_count += 1 + else: + track.pending_status = new_status + track.pending_count = 1 + if track.pending_count >= self.status_confirm_frames: + track.status = new_status + track.pending_status = None + track.pending_count = 0 + changed.append(track) + else: + track.pending_status = None + track.pending_count = 0 + + used.add(track.track_id) + + stale = [] + for tid, track in self.tracks.items(): + if tid not in used: + track.frames_missing += 1 + if track.frames_missing > self.max_missing: + stale.append(tid) + for tid in stale: + del self.tracks[tid] + + return created, changed + + def visible_tracks(self) -> List[Track]: + return [t for t in self.tracks.values() if t.frames_missing == 0] diff --git a/saqr/gui/__init__.py b/saqr/gui/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gui.py b/saqr/gui/app.py similarity index 76% rename from gui.py rename to saqr/gui/app.py index ad56a5b..147eba4 100644 --- a/gui.py +++ b/saqr/gui/app.py @@ -1,70 +1,43 @@ -""" -Saqr - PPE Safety Tracking | GUI Application -================================================= -PySide6 desktop GUI for real-time PPE compliance monitoring. - -Features: - - Live camera feed with PPE detection overlays - - Start / Stop / Source selection - - Real-time SAFE / PARTIAL / UNSAFE counters - - Track list with per-person status - - Event log panel - - Confidence & tracking parameter controls - - Capture gallery sidebar - - G1 robot camera support (RealSense / V4L2 /dev/videoX) - -Usage: - python gui.py - python gui.py --model models/saqr_best.pt - python gui.py --source 1 -""" - +"""Saqr PySide6 desktop GUI for live PPE compliance monitoring.""" from __future__ import annotations import sys import time -from pathlib import Path from datetime import datetime +from pathlib import Path from typing import Dict, List, Optional import cv2 import numpy as np -from PySide6.QtCore import Qt, QThread, Signal, Slot, QTimer, QSize -from PySide6.QtGui import QImage, QPixmap, QFont, QColor, QIcon +from PySide6.QtCore import Qt, QThread, Signal, Slot +from PySide6.QtGui import QColor, QImage, QPixmap from PySide6.QtWidgets import ( - QApplication, QMainWindow, QWidget, QLabel, QPushButton, - QVBoxLayout, QHBoxLayout, QGridLayout, QGroupBox, QComboBox, - QSlider, QSpinBox, QDoubleSpinBox, QTextEdit, QSplitter, - QFrame, QScrollArea, QFileDialog, QMessageBox, QStatusBar, + QApplication, QComboBox, QDoubleSpinBox, QFileDialog, QGridLayout, + QGroupBox, QHBoxLayout, QLabel, QMainWindow, QMessageBox, QPushButton, + QSpinBox, QTextEdit, QVBoxLayout, QWidget, ) - from ultralytics import YOLO -# Import Saqr core modules -from saqr import ( - collect_detections, group_detections_to_people, status_from_items, - split_wearing_missing, PersonTracker, EventLogger, Track, - save_track_image, emit_event, write_result_csv, draw_track, - draw_counters, setup_capture_dirs, resolve_model_path, - clamp_bbox, STATUSES, EVENTS_CSV, RESULT_CSV, ROOT, CAPTURES_DIR, - now_iso, -) - -from logger import get_logger +from saqr.core.capture import save_track_image, setup_capture_dirs +from saqr.core.compliance import split_wearing_missing +from saqr.core.detection import STATUSES, collect_detections +from saqr.core.drawing import draw_counters, draw_track +from saqr.core.events import EventLogger, emit_event, write_result_csv +from saqr.core.grouping import group_detections_to_people +from saqr.core.model import resolve_model_path +from saqr.core.paths import EVENTS_CSV, MODELS_DIR, PROJECT_ROOT, RESULT_CSV +from saqr.core.tracking import PersonTracker +from saqr.utils.logger import get_logger log = get_logger("Inference", "gui") -# ── Camera backends (from AI_Photographer patterns) ────────────────────────── def list_cameras(max_idx: int = 10) -> List[str]: - """Scan for available camera devices.""" - sources = [] - # V4L2 devices + sources: List[str] = [] for i in range(max_idx): dev = f"/dev/video{i}" if Path(dev).exists(): sources.append(dev) - # Fallback numeric indices if not sources: for i in range(4): cap = cv2.VideoCapture(i) @@ -75,7 +48,6 @@ def list_cameras(max_idx: int = 10) -> List[str]: def open_camera(source: str, width: int = 640, height: int = 480, fps: int = 30): - """Open camera with V4L2 backend and MJPEG codec (G1 compatible).""" if source.startswith("/dev/video"): cap = cv2.VideoCapture(source, cv2.CAP_V4L2) elif source.isdigit(): @@ -92,11 +64,10 @@ def open_camera(source: str, width: int = 640, height: int = 480, fps: int = 30) return cap -# ── Detection Worker Thread ─────────────────────────────────────────────────── class DetectionWorker(QThread): - frame_ready = Signal(np.ndarray, list) # annotated frame, visible tracks - event_fired = Signal(str) # event message string - stats_updated = Signal(dict) # {SAFE: n, PARTIAL: n, UNSAFE: n, fps: f} + frame_ready = Signal(np.ndarray, list) + event_fired = Signal(str) + stats_updated = Signal(dict) def __init__(self, parent=None): super().__init__(parent) @@ -116,8 +87,8 @@ class DetectionWorker(QThread): self.max_missing = max_missing self.match_distance = match_dist self.status_confirm = status_confirm - self.capture_dirs = setup_capture_dirs(ROOT) - if self.model is None or str(model_path) != getattr(self, '_last_model', ''): + self.capture_dirs = setup_capture_dirs() + if self.model is None or str(model_path) != getattr(self, "_last_model", ""): self.model = YOLO(model_path) self._last_model = str(model_path) @@ -166,7 +137,7 @@ class DetectionWorker(QThread): save_track_image(frame, track, self.capture_dirs) if track.track_id in event_ids: ev_type = "NEW" if track.track_id in created_ids else "STATUS_CHANGE" - wearing, missing, unknown = split_wearing_missing(track.items) + wearing, missing, _unknown = split_wearing_missing(track.items) msg = ( f"ID {track.track_id:04d} | {ev_type} | {track.status} | " f"W: {', '.join(wearing) or 'none'} | " @@ -176,7 +147,6 @@ class DetectionWorker(QThread): emit_event(track, event_logger, ev_type) draw_track(annotated, track) - # Write CSV periodically if frame_idx % 30 == 0: write_result_csv(list(tracker.tracks.values()), RESULT_CSV) @@ -190,7 +160,6 @@ class DetectionWorker(QThread): draw_counters(annotated, visible, fps) - # Emit signals counts = {s: 0 for s in STATUSES} for t in visible: counts[t.status] += 1 @@ -212,7 +181,6 @@ class DetectionWorker(QThread): self._running = False -# ── Helpers ─────────────────────────────────────────────────────────────────── def cv_to_qpixmap(frame: np.ndarray, max_w: int = 960, max_h: int = 720) -> QPixmap: h, w, ch = frame.shape rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) @@ -222,10 +190,8 @@ def cv_to_qpixmap(frame: np.ndarray, max_w: int = 960, max_h: int = 720) -> QPix Qt.TransformationMode.SmoothTransformation) -# ── Main Window ─────────────────────────────────────────────────────────────── class SaqrWindow(QMainWindow): - def __init__(self, default_model: str = "models/saqr_best.pt", - default_source: str = "0"): + def __init__(self, default_model: str = "saqr_best.pt", default_source: str = "0"): super().__init__() self.setWindowTitle("Saqr - PPE Safety Tracking") self.setMinimumSize(1200, 700) @@ -241,11 +207,8 @@ class SaqrWindow(QMainWindow): self.setCentralWidget(central) main_layout = QHBoxLayout(central) - # ── Left: Controls ──────────────────────────────────────────────── left = QVBoxLayout() - left.setMaximumWidth = 300 - # Model model_grp = QGroupBox("Model") model_lay = QVBoxLayout(model_grp) self.model_label = QLabel(self._default_model) @@ -256,7 +219,6 @@ class SaqrWindow(QMainWindow): model_lay.addWidget(btn_model) left.addWidget(model_grp) - # Camera cam_grp = QGroupBox("Camera Source") cam_lay = QVBoxLayout(cam_grp) self.cam_combo = QComboBox() @@ -266,7 +228,6 @@ class SaqrWindow(QMainWindow): cam_lay.addWidget(btn_refresh) left.addWidget(cam_grp) - # Parameters param_grp = QGroupBox("Parameters") param_lay = QGridLayout(param_grp) @@ -298,7 +259,6 @@ class SaqrWindow(QMainWindow): left.addWidget(param_grp) - # Start / Stop btn_lay = QHBoxLayout() self.btn_start = QPushButton("Start") self.btn_start.setStyleSheet("background-color: #2ecc71; color: white; font-weight: bold; padding: 8px;") @@ -311,7 +271,6 @@ class SaqrWindow(QMainWindow): btn_lay.addWidget(self.btn_stop) left.addLayout(btn_lay) - # Status counters stats_grp = QGroupBox("Live Status") stats_lay = QGridLayout(stats_grp) self.lbl_fps = QLabel("FPS: -") @@ -334,7 +293,6 @@ class SaqrWindow(QMainWindow): left.addStretch() - # ── Centre: Video feed ──────────────────────────────────────────── centre = QVBoxLayout() self.video_label = QLabel("No camera feed") self.video_label.setAlignment(Qt.AlignmentFlag.AlignCenter) @@ -344,7 +302,6 @@ class SaqrWindow(QMainWindow): self.video_label.setMinimumSize(640, 480) centre.addWidget(self.video_label) - # ── Right: Event log ────────────────────────────────────────────── right = QVBoxLayout() log_grp = QGroupBox("Event Log") log_lay = QVBoxLayout(log_grp) @@ -366,7 +323,6 @@ class SaqrWindow(QMainWindow): right.addWidget(log_grp) - # ── Assemble ────────────────────────────────────────────────────── left_widget = QWidget() left_widget.setLayout(left) left_widget.setFixedWidth(260) @@ -382,44 +338,38 @@ class SaqrWindow(QMainWindow): main_layout.addWidget(centre_widget, stretch=1) main_layout.addWidget(right_widget) - # Status bar self.statusBar().showMessage("Ready - load a model and start detection") def _scan_cameras(self): self.cam_combo.clear() sources = list_cameras() self.cam_combo.addItems(sources) - # Set default idx = self.cam_combo.findText(self._default_source) if idx >= 0: self.cam_combo.setCurrentIndex(idx) elif self.cam_combo.count() > 0: - # Try to add the default as custom self.cam_combo.addItem(self._default_source) self.cam_combo.setCurrentIndex(self.cam_combo.count() - 1) def _browse_model(self): path, _ = QFileDialog.getOpenFileName( - self, "Select YOLO Model", str(ROOT / "models"), "Model Files (*.pt)" + self, "Select YOLO Model", str(MODELS_DIR), "Model Files (*.pt)" ) if path: self.model_label.setText(path) def _start(self): - model_path = self.model_label.text() - if not Path(model_path).exists(): - # Try relative to ROOT - full = ROOT / model_path - if not full.exists(): - QMessageBox.critical(self, "Error", f"Model not found:\n{model_path}") - return - model_path = str(full) + try: + model_path = resolve_model_path(self.model_label.text()) + except FileNotFoundError as e: + QMessageBox.critical(self, "Error", str(e)) + return source = self.cam_combo.currentText() self.worker = DetectionWorker() self.worker.configure( - model_path=model_path, + model_path=str(model_path), source=source, conf=self.conf_spin.value(), max_missing=self.missing_spin.value(), @@ -446,12 +396,12 @@ class SaqrWindow(QMainWindow): self.statusBar().showMessage("Stopped") @Slot(np.ndarray, list) - def _on_frame(self, frame: np.ndarray, visible: list): + def _on_frame(self, frame, visible): pix = cv_to_qpixmap(frame, self.video_label.width(), self.video_label.height()) self.video_label.setPixmap(pix) @Slot(str) - def _on_event(self, msg: str): + def _on_event(self, msg): ts = datetime.now().strftime("%H:%M:%S") color = "#c9d1d9" if "UNSAFE" in msg: @@ -462,15 +412,13 @@ class SaqrWindow(QMainWindow): color = "#d29922" elif "ERROR" in msg: color = "#f85149" - self.event_log.append(f'[{ts}] {msg}') - # Auto-scroll self.event_log.verticalScrollBar().setValue( self.event_log.verticalScrollBar().maximum() ) @Slot(dict) - def _on_stats(self, stats: dict): + def _on_stats(self, stats): self.lbl_fps.setText(f"FPS: {stats.get('fps', 0):.1f}") self.lbl_safe.setText(f"SAFE: {stats.get('SAFE', 0)}") self.lbl_partial.setText(f"PARTIAL: {stats.get('PARTIAL', 0)}") @@ -484,11 +432,12 @@ class SaqrWindow(QMainWindow): def _export_csv(self): path, _ = QFileDialog.getSaveFileName( - self, "Export CSV", str(ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"), + self, "Export CSV", + str(PROJECT_ROOT / f"ppe_report_{datetime.now().strftime('%Y%m%d_%H%M%S')}.csv"), "CSV Files (*.csv)" ) if path: - from manager import export_csv, load_photos + from saqr.apps.manager_cli import export_csv, load_photos export_csv(load_photos(), Path(path)) self._on_event(f"Exported: {path}") @@ -497,18 +446,16 @@ class SaqrWindow(QMainWindow): event.accept() -# ── Entry point ─────────────────────────────────────────────────────────────── def main(): import argparse parser = argparse.ArgumentParser(description="Saqr PPE GUI") - parser.add_argument("--model", default="models/saqr_best.pt") + parser.add_argument("--model", default="saqr_best.pt") parser.add_argument("--source", default="0") args = parser.parse_args() app = QApplication(sys.argv) app.setStyle("Fusion") - # Dark theme from PySide6.QtGui import QPalette palette = QPalette() palette.setColor(QPalette.ColorRole.Window, QColor(30, 30, 46)) diff --git a/saqr/robot/__init__.py b/saqr/robot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/saqr/robot/bridge.py b/saqr/robot/bridge.py new file mode 100644 index 0000000..75022d9 --- /dev/null +++ b/saqr/robot/bridge.py @@ -0,0 +1,396 @@ +"""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\d+)\s*\|\s*" + r"(?PNEW|STATUS_CHANGE)\s*\|\s*" + r"(?PSAFE|PARTIAL|UNSAFE)\s*\|\s*" + r"wearing:\s*(?P[^|]*?)\s*\|\s*" + r"missing:\s*(?P[^|]*?)\s*\|\s*" + r"unknown:\s*(?P.*?)\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() diff --git a/controller.py b/saqr/robot/controller.py similarity index 100% rename from controller.py rename to saqr/robot/controller.py diff --git a/saqr/robot/robot_controller.py b/saqr/robot/robot_controller.py new file mode 100644 index 0000000..03014bc --- /dev/null +++ b/saqr/robot/robot_controller.py @@ -0,0 +1,216 @@ +"""G1 arm + audio + LowState DDS client owned by the bridge. + +A dedicated TTS worker thread paces ``TtsMaker`` calls so overlapping phrases +don't trip the SDK's "device busy" error (3104). The busy multiplier adapts +up on 3104s and decays on clean calls. +""" +from __future__ import annotations + +import collections +import datetime +import threading +import time +from typing import Deque, Optional + +TTS_VOLUME = 100 + +TTS_SECONDS_PER_CHAR = 0.12 +TTS_MIN_SECONDS = 2.5 +TTS_QUEUE_MAX = 4 +TTS_BUSY_FACTOR_MIN = 1.0 +TTS_BUSY_FACTOR_MAX = 2.5 +TTS_BUSY_FACTOR_UP = 1.20 +TTS_BUSY_FACTOR_DOWN = 0.97 + +REJECT_ACTION = "reject" +RELEASE_ACTION = "release arm" + + +def _ts() -> str: + return datetime.datetime.now().strftime("%H:%M:%S.%f")[:-3] + + +class RobotController: + """Owns both the G1 arm action client and the G1 audio (TTS) client. + + A single ``ChannelFactoryInitialize`` call is shared by both clients and + the optional ``rt/lowstate`` subscriber used by the wireless-remote loop. + """ + + def __init__(self, iface: Optional[str], 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 + + 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) + return + + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + from unitree_sdk2py.g1.arm.g1_arm_action_client import ( + G1ArmActionClient, + action_map, + ) + from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient + + self._action_map = action_map + + if iface: + ChannelFactoryInitialize(0, iface) + else: + ChannelFactoryInitialize(0) + + self.arm_client = G1ArmActionClient() + self.arm_client.SetTimeout(timeout) + self.arm_client.Init() + print(f"[BRIDGE] G1ArmActionClient ready (iface={iface or 'default'})", + flush=True) + + self.audio_client = AudioClient() + self.audio_client.SetTimeout(timeout) + self.audio_client.Init() + try: + self.audio_client.SetVolume(TTS_VOLUME) + except Exception as e: + print(f"[BRIDGE][WARN] AudioClient.SetVolume failed: {e}", flush=True) + 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 saqr.robot.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 — enqueue the phrase for the worker thread.""" + 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 + if self._tts_queue and self._tts_queue[-1] == text: + return + self._tts_queue.append(text) + self._tts_event.set() + + def shutdown_tts(self): + 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): + 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: + code = self.audio_client.TtsMaker(text, self.tts_speaker_id) + except Exception as e: + 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() + + remaining = est - call_dt + if remaining > 0: + time.sleep(remaining) + + # ── Arm ───────────────────────────────────────────────────────────────── + def reject(self, release_after: float): + if self.dry_run: + print(f"[BRIDGE] (dry) would run '{REJECT_ACTION}' " + f"then release after {release_after:.1f}s", flush=True) + return + if self.arm_client is None or self._action_map is None: + return + if REJECT_ACTION not in self._action_map: + print(f"[BRIDGE][ERR] '{REJECT_ACTION}' not in SDK action_map", + flush=True) + return + print(f"[BRIDGE] -> {REJECT_ACTION}", flush=True) + self.arm_client.ExecuteAction(self._action_map[REJECT_ACTION]) + if release_after > 0: + time.sleep(release_after) + print(f"[BRIDGE] -> {RELEASE_ACTION}", flush=True) + self.arm_client.ExecuteAction(self._action_map[RELEASE_ACTION]) diff --git a/saqr/utils/__init__.py b/saqr/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/logger.py b/saqr/utils/logger.py similarity index 85% rename from logger.py rename to saqr/utils/logger.py index 63a8a7d..06d6d9e 100644 --- a/logger.py +++ b/saqr/utils/logger.py @@ -2,16 +2,15 @@ from __future__ import annotations import json import logging -from pathlib import Path from typing import Dict +from saqr.core.paths import CONFIG_DIR, LOGS_DIR + _LOGGER_CACHE: Dict[str, logging.Logger] = {} -_ROOT = Path(__file__).resolve().parent - def _load_log_cfg() -> dict: - cfg_path = _ROOT / "Config" / "logging.json" + cfg_path = CONFIG_DIR / "logging.json" try: with open(cfg_path, "r") as f: return json.load(f) @@ -24,14 +23,14 @@ def _level_from_name(name: str) -> int: def get_logger(category: str, name: str) -> logging.Logger: - """Return a cached logger that writes to Logs//.log.""" + """Return a cached logger that writes to runtime/logs//.log.""" key = f"{category}.{name}" if key in _LOGGER_CACHE: return _LOGGER_CACHE[key] log_cfg = _load_log_cfg() - log_dir = _ROOT / "Logs" / category + log_dir = LOGS_DIR / category log_dir.mkdir(parents=True, exist_ok=True) logger = logging.getLogger(key) diff --git a/saqr_g1_bridge.py b/saqr_g1_bridge.py deleted file mode 100644 index f1b5a7d..0000000 --- a/saqr_g1_bridge.py +++ /dev/null @@ -1,727 +0,0 @@ -#!/usr/bin/env python3 -""" -saqr_g1_bridge.py - -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.py 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. - -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: - # 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 --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 -""" - -from __future__ import annotations - -import argparse -import collections -import datetime -import os -import re -import signal -import subprocess -import sys -import threading -import time -from pathlib import Path -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 ───────────────────────────────────────────────────────────────── -HERE = Path(__file__).resolve().parent -SAQR_DIR = HERE # bridge lives next to saqr.py -SAQR_SCRIPT = SAQR_DIR / "saqr.py" - -DANGER_STATUS = "UNSAFE" -SAFE_STATUS = "SAFE" -REJECT_ACTION = "reject" -RELEASE_ACTION = "release arm" - -# G1 onboard TtsMaker (see Project/Sanad/voice_example.py mode 6). -# speaker_id=2 was confirmed English on current G1 firmware. -TTS_SPEAKER_ID = 2 -TTS_VOLUME = 100 - -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." -) -# 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( - r"^ID\s+(?P\d+)\s*\|\s*" - r"(?PNEW|STATUS_CHANGE)\s*\|\s*" - r"(?PSAFE|PARTIAL|UNSAFE)\s*\|\s*" - r"wearing:\s*(?P[^|]*?)\s*\|\s*" - r"missing:\s*(?P[^|]*?)\s*\|\s*" - r"unknown:\s*(?P.*?)\s*$" -) - - -def _parse_list_field(s: str) -> list: - """Parse 'helmet, vest' or 'none' into a list of items.""" - 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: - """Join a list in natural English: 'helmet and vest', 'a, b, and c'.""" - 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)) - - -# ── G1 robot controller (lazy import: SDK only loaded when not in dry-run) ─── -class RobotController: - """Owns both the G1 arm action client and the G1 audio (TTS) client. - - A single ChannelFactoryInitialize call is shared by both clients. - """ - - def __init__( - self, - iface: Optional[str], - 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) - return - - from unitree_sdk2py.core.channel import ChannelFactoryInitialize - from unitree_sdk2py.g1.arm.g1_arm_action_client import ( - G1ArmActionClient, - action_map, - ) - from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient - - self._action_map = action_map - - if iface: - ChannelFactoryInitialize(0, iface) - else: - ChannelFactoryInitialize(0) - - self.arm_client = G1ArmActionClient() - self.arm_client.SetTimeout(timeout) - self.arm_client.Init() - print(f"[BRIDGE] G1ArmActionClient ready (iface={iface or 'default'})", - flush=True) - - self.audio_client = AudioClient() - self.audio_client.SetTimeout(timeout) - self.audio_client.Init() - try: - self.audio_client.SetVolume(TTS_VOLUME) - except Exception as e: - print(f"[BRIDGE][WARN] AudioClient.SetVolume failed: {e}", flush=True) - 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: - code = self.audio_client.TtsMaker(text, self.tts_speaker_id) - except Exception as e: - 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): - if self.dry_run: - print(f"[BRIDGE] (dry) would run '{REJECT_ACTION}' " - f"then release after {release_after:.1f}s", flush=True) - return - if self.arm_client is None or self._action_map is None: - return - if REJECT_ACTION not in self._action_map: - print(f"[BRIDGE][ERR] '{REJECT_ACTION}' not in SDK action_map", - flush=True) - return - print(f"[BRIDGE] -> {REJECT_ACTION}", flush=True) - self.arm_client.ExecuteAction(self._action_map[REJECT_ACTION]) - if release_after > 0: - time.sleep(release_after) - print(f"[BRIDGE] -> {RELEASE_ACTION}", flush=True) - self.arm_client.ExecuteAction(self._action_map[RELEASE_ACTION]) - - -# ── 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.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: - return - # Always echo Saqr output so the user still sees the live stream. - 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 - - # Only SAFE / UNSAFE transitions trigger the robot. PARTIAL is silent. - if status not in (DANGER_STATUS, SAFE_STATUS): - return - - # Only fire on transitions, not on every NEW/STATUS_CHANGE for the - # same status. - 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 - - # Run robot actions outside the lock so we don't block parsing. - try: - if status == DANGER_STATUS: - self.robot.speak(build_unsafe_tts(missing)) - self.robot.reject(release_after=self.release_after_s) - else: # SAFE - self.robot.speak(TTS_TEXT_SAFE) - except Exception as e: - print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True) - - -# ── 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). - return [sys.executable, "-u", str(SAQR_SCRIPT), *saqr_extra_args] - - -def split_argv(argv: list[str]) -> tuple[list[str], list[str]]: - """Split bridge args from saqr passthrough args at the first '--'.""" - if "--" in argv: - idx = argv.index("--") - return argv[:idx], argv[idx + 1 :] - return argv, [] - - -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. enp3s0). Optional.") - ap.add_argument("--timeout", type=float, default=10.0, - help="G1 arm client timeout (seconds).") - ap.add_argument("--cooldown", type=float, default=8.0, - help="Per-track-id seconds before reject can re-trigger.") - ap.add_argument("--release-after", type=float, default=2.0, - help="Seconds before auto-running 'release arm' (0 = never).") - ap.add_argument("--dry-run", action="store_true", - 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, - help="Saqr --source (0/realsense/path). Default: leave to saqr.") - ap.add_argument("--headless", action="store_true", - help="Pass --headless to saqr.") - ap.add_argument("--saqr-conf", type=float, default=None, - help="Pass --conf to saqr.") - ap.add_argument("--imgsz", type=int, default=None, - help="Pass --imgsz to saqr.") - ap.add_argument("--device", default=None, - help="Pass --device to saqr (e.g. cpu / 0 / cuda:0).") - - args = ap.parse_args(bridge_argv) - - # Build saqr args from convenience flags + raw passthrough. - saqr_args: list[str] = [] - 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, - ) - 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} -> 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: - 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: - # 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__": - main() diff --git a/scripts/deploy.sh b/scripts/deploy.sh new file mode 100755 index 0000000..c6786b4 --- /dev/null +++ b/scripts/deploy.sh @@ -0,0 +1,106 @@ +#!/bin/bash +# ============================================================================ +# Saqr PPE Detection - Deploy to Unitree G1 +# ============================================================================ +# +# Usage (from dev machine, any cwd): +# scripts/deploy.sh # deploy + install deps +# scripts/deploy.sh --run # deploy + install + start bridge +# scripts/deploy.sh --ip 10.0.0.5 # custom robot IP +# +# ============================================================================ + +set -e + +HERE="$(cd "$(dirname "$0")" && pwd)" +PROJECT_ROOT="$(cd "$HERE/.." && pwd)" + +ROBOT_IP="${ROBOT_IP:-192.168.123.164}" +ROBOT_USER="${ROBOT_USER:-unitree}" +ROBOT_ENV="${ROBOT_ENV:-marcus}" +REMOTE_DIR="/home/${ROBOT_USER}/Saqr" +SSH_OPTS="-o StrictHostKeyChecking=no -o ConnectTimeout=10" + +RUN_AFTER=false + +while [[ $# -gt 0 ]]; do + case $1 in + --run) RUN_AFTER=true; shift ;; + --ip) ROBOT_IP="$2"; shift 2 ;; + --env) ROBOT_ENV="$2"; shift 2 ;; + *) echo "Unknown arg: $1"; exit 1 ;; + esac +done + +echo "============================================" +echo " Saqr PPE - Deploy to Unitree G1" +echo "============================================" +echo " Robot : ${ROBOT_USER}@${ROBOT_IP}" +echo " Env : ${ROBOT_ENV}" +echo " Remote: ${REMOTE_DIR}" +echo " Source: ${PROJECT_ROOT}" +echo "============================================" + +echo "" +echo "[1/4] Testing SSH connection..." +ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} "echo 'Connected OK'" || { + echo "[ERROR] Cannot reach ${ROBOT_IP}. Is the robot on?" + exit 1 +} + +echo "[2/4] Creating remote directory..." +ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} \ + "mkdir -p ${REMOTE_DIR}/runtime/{captures/{SAFE,PARTIAL,UNSAFE},logs,runs} ${REMOTE_DIR}/data/models" + +echo "[3/4] Rsyncing package..." +RSYNC_OPTS=( + -avz --delete + --exclude="__pycache__" --exclude="*.pyc" + --exclude=".git" --exclude="runtime" --exclude="data/dataset" + --exclude="*.egg-info" --exclude="build/" --exclude="dist/" +) +rsync "${RSYNC_OPTS[@]}" \ + "${PROJECT_ROOT}/saqr" \ + "${PROJECT_ROOT}/scripts" \ + "${PROJECT_ROOT}/config" \ + "${PROJECT_ROOT}/docs" \ + "${PROJECT_ROOT}/pyproject.toml" \ + "${PROJECT_ROOT}/requirements.txt" \ + "${PROJECT_ROOT}/README.md" \ + ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/ + +# Weights (these are outside the package so rsync them separately). +if [ -f "${PROJECT_ROOT}/data/models/saqr_best.pt" ]; then + echo " Uploading saqr_best.pt..." + scp ${SSH_OPTS} "${PROJECT_ROOT}/data/models/saqr_best.pt" \ + ${ROBOT_USER}@${ROBOT_IP}:${REMOTE_DIR}/data/models/ +else + echo " [WARN] data/models/saqr_best.pt not found - train first!" +fi + +echo "[4/4] Installing package on robot..." +ssh ${SSH_OPTS} ${ROBOT_USER}@${ROBOT_IP} bash -s </dev/null || pip install -e . + chmod +x scripts/start_saqr.sh scripts/run_robot.sh scripts/run_local.sh 2>/dev/null || true + echo " Install OK" +INSTALL_EOF + +if [ "$RUN_AFTER" = true ]; then + echo "" + echo "Starting Saqr bridge on robot..." + ssh ${SSH_OPTS} -t ${ROBOT_USER}@${ROBOT_IP} bash -c "'${REMOTE_DIR}/scripts/start_saqr.sh'" +else + echo "" + echo "============================================" + echo " Deployed! Start the bridge via:" + echo "============================================" + echo "" + echo " ssh ${ROBOT_USER}@${ROBOT_IP}" + echo " ${REMOTE_DIR}/scripts/start_saqr.sh # manual" + echo " sudo systemctl restart saqr-bridge # systemd (see docs/start.md)" + echo "" +fi diff --git a/scripts/run_local.sh b/scripts/run_local.sh new file mode 100755 index 0000000..4393321 --- /dev/null +++ b/scripts/run_local.sh @@ -0,0 +1,87 @@ +#!/bin/bash +# ============================================================================ +# Saqr PPE Detection - Run on Local Laptop +# ============================================================================ +# +# Usage: +# scripts/run_local.sh # webcam 0 +# scripts/run_local.sh --source 1 # webcam 1 +# scripts/run_local.sh --source video.mp4 # video file +# scripts/run_local.sh --gui # PySide6 GUI +# scripts/run_local.sh --detect # simple detection (no tracking) +# +# ============================================================================ + +set -e + +HERE="$(cd "$(dirname "$0")" && pwd)" +PROJECT_ROOT="$(cd "$HERE/.." && pwd)" +cd "$PROJECT_ROOT" + +SOURCE="0" +MODEL="saqr_best.pt" +CONF="0.35" +MODE="saqr" # saqr | gui | detect +HEADLESS=false +MAX_MISSING=90 +MATCH_DIST=250 +CONFIRM=5 + +while [[ $# -gt 0 ]]; do + case $1 in + --source) SOURCE="$2"; shift 2 ;; + --model) MODEL="$2"; shift 2 ;; + --conf) CONF="$2"; shift 2 ;; + --gui) MODE="gui"; shift ;; + --detect) MODE="detect"; shift ;; + --headless) HEADLESS=true; shift ;; + --max-missing) MAX_MISSING="$2"; shift 2 ;; + --match-distance) MATCH_DIST="$2"; shift 2 ;; + --confirm) CONFIRM="$2"; shift 2 ;; + *) echo "Unknown arg: $1"; exit 1 ;; + esac +done + +if command -v conda &>/dev/null; then + source "$(conda info --base)/etc/profile.d/conda.sh" 2>/dev/null || true + conda activate AI_MSI_yolo 2>/dev/null || true +fi + +echo "============================================" +echo " Saqr PPE Detection - Local Laptop" +echo "============================================" +echo " Mode : $MODE" +echo " Source : $SOURCE" +echo " Model : $MODEL" +echo " Conf : $CONF" +echo "============================================" +echo "" + +HEADLESS_FLAG="" +if [ "$HEADLESS" = true ]; then + HEADLESS_FLAG="--headless" +fi + +case $MODE in + saqr) + python -m saqr.apps.saqr_cli \ + --source "$SOURCE" \ + --model "$MODEL" \ + --conf "$CONF" \ + --max-missing "$MAX_MISSING" \ + --match-distance "$MATCH_DIST" \ + --status-confirm-frames "$CONFIRM" \ + $HEADLESS_FLAG + ;; + gui) + python -m saqr.gui.app \ + --source "$SOURCE" \ + --model "$MODEL" + ;; + detect) + python -m saqr.apps.detect_cli \ + --source "$SOURCE" \ + --model "$MODEL" \ + --conf "$CONF" + ;; +esac diff --git a/scripts/run_robot.sh b/scripts/run_robot.sh new file mode 100755 index 0000000..16083ab --- /dev/null +++ b/scripts/run_robot.sh @@ -0,0 +1,102 @@ +#!/bin/bash +# ============================================================================ +# Saqr PPE Detection - Run on Unitree G1 Robot (no bridge, direct saqr run) +# ============================================================================ +# +# Run on the robot's physical terminal (with monitor) or via ssh -X: +# scripts/run_robot.sh +# scripts/run_robot.sh --headless # no display +# scripts/run_robot.sh --source /dev/video2 # V4L2 fallback +# +# For the production R2+X / R2+Y workflow, use scripts/start_saqr.sh instead. +# ============================================================================ + +set -e + +HERE="$(cd "$(dirname "$0")" && pwd)" +PROJECT_ROOT="$(cd "$HERE/.." && pwd)" +cd "$PROJECT_ROOT" + +SOURCE="realsense" +MODEL="saqr_best.pt" +CONF="0.35" +HEADLESS=false +MAX_MISSING=120 +MATCH_DIST=300 +CONFIRM=7 +DEVICE="0" +IMGSZ=320 +HALF=true +STREAM_PORT=0 + +while [[ $# -gt 0 ]]; do + case $1 in + --source) SOURCE="$2"; shift 2 ;; + --model) MODEL="$2"; shift 2 ;; + --conf) CONF="$2"; shift 2 ;; + --headless) HEADLESS=true; shift ;; + --max-missing) MAX_MISSING="$2"; shift 2 ;; + --match-distance) MATCH_DIST="$2"; shift 2 ;; + --confirm) CONFIRM="$2"; shift 2 ;; + --device) DEVICE="$2"; shift 2 ;; + --imgsz) IMGSZ="$2"; shift 2 ;; + --no-half) HALF=false; shift ;; + --stream) STREAM_PORT="$2"; shift 2 ;; + --cpu) DEVICE="cpu"; HALF=false; shift ;; + *) echo "Unknown arg: $1"; exit 1 ;; + esac +done + +source ~/miniconda3/etc/profile.d/conda.sh 2>/dev/null || true +conda activate marcus 2>/dev/null || conda activate teleimager 2>/dev/null || true + +YEAR=$(date +%Y) +if [ "$YEAR" -lt 2025 ]; then + echo "[WARN] System clock is wrong (year=$YEAR). Fixing..." + echo "123" | sudo -S date -s "2026-04-10 16:00:00" 2>/dev/null || true +fi + +if [ "$HEADLESS" = true ]; then + export QT_QPA_PLATFORM=offscreen + HEADLESS_FLAG="--headless" + echo "Mode: HEADLESS (no display, results saved to runtime/captures/)" +else + xhost + >/dev/null 2>&1 || true + export DISPLAY=:0 + HEADLESS_FLAG="" + echo "Mode: DISPLAY (OpenCV window on monitor)" +fi + +HALF_FLAG="" +if [ "$HALF" = true ]; then + HALF_FLAG="--half" +fi + +STREAM_FLAG="" +if [ "$STREAM_PORT" -gt 0 ]; then + STREAM_FLAG="--stream $STREAM_PORT" +fi + +echo "============================================" +echo " Saqr PPE Detection - Unitree G1 Robot" +echo "============================================" +echo " Source : $SOURCE" +echo " Model : $MODEL" +echo " Device : $DEVICE (half=$HALF, imgsz=$IMGSZ)" +echo " Conf : $CONF" +echo " Stream : ${STREAM_PORT:-disabled}" +echo "============================================" +echo "" + +python -m saqr.apps.saqr_cli \ + --source "$SOURCE" \ + --model "$MODEL" \ + --conf "$CONF" \ + --max-missing "$MAX_MISSING" \ + --match-distance "$MATCH_DIST" \ + --status-confirm-frames "$CONFIRM" \ + --device "$DEVICE" \ + --imgsz "$IMGSZ" \ + $HALF_FLAG \ + $STREAM_FLAG \ + $HEADLESS_FLAG diff --git a/scripts/saqr-bridge.service b/scripts/saqr-bridge.service new file mode 100644 index 0000000..8f13b65 --- /dev/null +++ b/scripts/saqr-bridge.service @@ -0,0 +1,22 @@ +[Unit] +Description=Saqr PPE bridge for Unitree G1 +After=network-online.target +Wants=network-online.target + +[Service] +Type=simple +User=unitree +WorkingDirectory=/home/unitree/Saqr +ExecStart=/home/unitree/Saqr/scripts/start_saqr.sh +Restart=on-failure +RestartSec=5 +StandardOutput=journal +StandardError=journal + +# Add Environment= lines here to override SAQR_SOURCE, STREAM_PORT, DDS_IFACE, etc. +# Example: +# Environment=SAQR_SOURCE=/dev/video2 +# Environment=STREAM_PORT=9090 + +[Install] +WantedBy=multi-user.target diff --git a/scripts/start_saqr.sh b/scripts/start_saqr.sh new file mode 100755 index 0000000..0f65038 --- /dev/null +++ b/scripts/start_saqr.sh @@ -0,0 +1,65 @@ +#!/bin/bash +# ============================================================================ +# start_saqr.sh — boot launcher for the Saqr / G1 bridge. +# ============================================================================ +# +# What it does: +# 1. Sources miniconda and activates the target env (default: marcus). +# 2. cd to the project root (parent of this scripts/ dir). +# 3. Execs `python -m saqr.robot.bridge` 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 +# +# Designed to be run by systemd at boot — see saqr-bridge.service. +# Can also be run manually: scripts/start_saqr.sh +# ============================================================================ + +set -u + +HERE="$(cd "$(dirname "$0")" && pwd)" +SAQR_DIR="${SAQR_DIR:-$(cd "$HERE/.." && pwd)}" + +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}" + +if [ ! -d "$SAQR_DIR" ]; then + echo "[start_saqr] FATAL: SAQR_DIR not found: $SAQR_DIR" >&2 + exit 1 +fi + +if [ ! -d "$SAQR_DIR/saqr" ]; then + echo "[start_saqr] FATAL: saqr/ package 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 + +# 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 python3 -m saqr.robot.bridge \ + --iface "$DDS_IFACE" \ + --source "$SAQR_SOURCE" \ + --headless \ + -- --stream "$STREAM_PORT" diff --git a/start.md b/start.md deleted file mode 100644 index 55e16e8..0000000 --- a/start.md +++ /dev/null @@ -1,186 +0,0 @@ -# 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 deleted file mode 100755 index 1c7b039..0000000 --- a/start_saqr.sh +++ /dev/null @@ -1,70 +0,0 @@ -#!/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"