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

Update 2026-04-19 23:50:47

Browse Source
This commit is contained in:
kassam 2026-04-19 23:50:50 +04:00
parent 7cd8d78c3e
commit a94d20ab15
49 changed files with 2652 additions and 3032 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
.gitignore vendored
View File

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

564
DEPLOY.md
View File

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

51
README.md Normal file
View File

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

0
Config/logging.json → config/logging.json
View File

129
deploy.sh
View File

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

322
docs/DEPLOY.md Normal file
View File

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

169
docs/start.md Normal file
View File

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

0
use case catalogue.pdf → docs/use_case_catalogue.pdf
View File

31
pyproject.toml Normal file
View File

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

110
run_local.sh
View File

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

116
run_robot.sh
View File

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

909
saqr.py
View File

@ -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'<html><body style="margin:0;background:#000">'
b'<img src="/stream" style="width:100%;height:auto">'
b'</body></html>')
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()

3
saqr/__init__.py Normal file
View File

@ -0,0 +1,3 @@
"""Saqr — PPE safety detection and G1 humanoid integration."""
__version__ = "0.1.0"

0
saqr/apps/__init__.py Normal file
View File

56
detect.py → saqr/apps/detect_cli.py
View File

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

63
manager.py → saqr/apps/manager_cli.py
View File

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

193
saqr/apps/saqr_cli.py Normal file
View File

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

51
train.py → saqr/apps/train_cli.py
View File

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

18
view_stream.py → saqr/apps/view_stream.py
View File

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

0
saqr/core/__init__.py Normal file
View File

88
saqr/core/camera.py Normal file
View File

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

41
saqr/core/capture.py Normal file
View File

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

36
saqr/core/compliance.py Normal file
View File

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

56
saqr/core/detection.py Normal file
View File

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

64
saqr/core/drawing.py Normal file
View File

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

93
saqr/core/events.py Normal file
View File

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

35
saqr/core/geometry.py Normal file
View File

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

80
saqr/core/grouping.py Normal file
View File

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

20
saqr/core/model.py Normal file
View File

@ -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/<basename>."""
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}")

19
saqr/core/paths.py Normal file
View File

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

40
saqr/core/pipeline.py Normal file
View File

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

62
saqr/core/streaming.py Normal file
View File

@ -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'<html><body style="margin:0;background:#000">'
b'<img src="/stream" style="width:100%;height:auto">'
b'</body></html>')
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()

118
saqr/core/tracking.py Normal file
View File

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

0
saqr/gui/__init__.py Normal file
View File

131
gui.py → saqr/gui/app.py
View File

@ -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'<span style="color:{color}">[{ts}] {msg}</span>')
# 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))

0
saqr/robot/__init__.py Normal file
View File

396
saqr/robot/bridge.py Normal file
View File

@ -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<id>\d+)\s*\|\s*"
r"(?P<event>NEW|STATUS_CHANGE)\s*\|\s*"
r"(?P<status>SAFE|PARTIAL|UNSAFE)\s*\|\s*"
r"wearing:\s*(?P<wearing>[^|]*?)\s*\|\s*"
r"missing:\s*(?P<missing>[^|]*?)\s*\|\s*"
r"unknown:\s*(?P<unknown>.*?)\s*$"
)
def _parse_list_field(s: str) -> list:
s = (s or "").strip()
if not s or s.lower() == "none":
return []
return [x.strip() for x in s.split(",") if x.strip()]
def _human_join(items: list) -> str:
if not items:
return ""
if len(items) == 1:
return items[0]
if len(items) == 2:
return f"{items[0]} and {items[1]}"
return ", ".join(items[:-1]) + f", and {items[-1]}"
def build_unsafe_tts(missing: list) -> str:
if not missing:
return TTS_UNSAFE_GENERIC
return TTS_UNSAFE_WITH_MISSING.format(items=_human_join(missing))
def build_saqr_cmd(saqr_extra_args: list) -> list:
"""Invoke the saqr CLI via ``python -m`` so it picks up the package layout."""
return [sys.executable, "-u", "-m", "saqr.apps.saqr_cli", *saqr_extra_args]
def split_argv(argv):
if "--" in argv:
idx = argv.index("--")
return argv[:idx], argv[idx + 1:]
return argv, []
class Bridge:
"""Owns the saqr subprocess lifecycle and the event-stream parser."""
def __init__(self, robot: RobotController, cooldown_s: float,
release_after_s: float, saqr_args: list,
env: Dict[str, str], cwd: str):
self.robot = robot
self.cooldown_s = cooldown_s
self.release_after_s = release_after_s
self.saqr_args = saqr_args
self.env = env
self.cwd = cwd
self.last_status: Dict[int, str] = {}
self.last_trigger_t: Dict[tuple, float] = {}
self._state_lock = threading.Lock()
self.proc: Optional[subprocess.Popen] = None
self.reader_thread: Optional[threading.Thread] = None
self._proc_lock = threading.Lock()
self._proc_start_t: float = 0.0
def is_running(self) -> bool:
with self._proc_lock:
return self.proc is not None and self.proc.poll() is None
def start_saqr(self):
with self._proc_lock:
if self.proc is not None and self.proc.poll() is None:
print("[BRIDGE] start ignored — saqr already running", flush=True)
return
cmd = build_saqr_cmd(self.saqr_args)
print(f"[BRIDGE] starting saqr: {' '.join(cmd)}", flush=True)
self.proc = subprocess.Popen(
cmd,
cwd=self.cwd,
stdout=subprocess.PIPE,
stderr=subprocess.STDOUT,
bufsize=1,
text=True,
env=self.env,
)
self._proc_start_t = time.time()
with self._state_lock:
self.last_status.clear()
self.last_trigger_t.clear()
self.reader_thread = threading.Thread(
target=self._read_stdout, args=(self.proc,), daemon=True,
)
self.reader_thread.start()
def stop_saqr(self):
with self._proc_lock:
proc = self.proc
if proc is None or proc.poll() is not None:
print("[BRIDGE] stop ignored — saqr not running", flush=True)
self.proc = None
return
print("[BRIDGE] stopping saqr (SIGINT)", flush=True)
try:
proc.send_signal(signal.SIGINT)
except Exception:
pass
try:
proc.wait(timeout=3.0)
except subprocess.TimeoutExpired:
print("[BRIDGE] saqr did not exit in 3s, sending SIGTERM", flush=True)
try:
proc.terminate()
proc.wait(timeout=2.0)
except subprocess.TimeoutExpired:
print("[BRIDGE] saqr unresponsive, sending SIGKILL", flush=True)
proc.kill()
proc.wait()
if self.reader_thread is not None:
self.reader_thread.join(timeout=2.0)
with self._proc_lock:
self.proc = None
self.reader_thread = None
self.robot.speak(TTS_BRIDGE_DEACTIVATED)
def _read_stdout(self, proc: subprocess.Popen):
start_t = self._proc_start_t
try:
assert proc.stdout is not None
for line in proc.stdout:
self.handle_line(line)
except Exception as e:
print(f"[BRIDGE][ERR] reader thread: {e}", flush=True)
rc = proc.wait()
lifetime = time.time() - start_t if start_t > 0 else 0.0
print(f"[BRIDGE] saqr exited rc={rc} (lifetime={lifetime:.1f}s)",
flush=True)
if rc not in (0, -2) and 0 < lifetime < QUICK_FAIL_WINDOW_S:
try:
self.robot.speak(TTS_BRIDGE_NO_CAMERA)
except Exception as e:
print(f"[BRIDGE][ERR] no-camera tts failed: {e}", flush=True)
def handle_line(self, line: str):
line = line.rstrip()
if not line:
return
print(line, flush=True)
m = EVENT_RE.match(line)
if not m:
return
track_id = int(m.group("id"))
status = m.group("status")
missing = _parse_list_field(m.group("missing"))
with self._state_lock:
prev = self.last_status.get(track_id)
self.last_status[track_id] = status
if status not in (DANGER_STATUS, SAFE_STATUS):
return
if prev == status:
return
now = time.time()
last_t = self.last_trigger_t.get((track_id, status), 0.0)
if (now - last_t) < self.cooldown_s:
return
self.last_trigger_t[(track_id, status)] = now
try:
if status == DANGER_STATUS:
self.robot.speak(build_unsafe_tts(missing))
self.robot.reject(release_after=self.release_after_s)
else:
self.robot.speak(TTS_TEXT_SAFE)
except Exception as e:
print(f"[BRIDGE][ERR] robot action failed: {e}", flush=True)
def trigger_loop(bridge: Bridge, hub, stop_event: threading.Event,
poll_hz: float = 50.0):
"""Watch the wireless remote for R2+X (start) and R2+Y (stop)."""
period = 1.0 / max(poll_hz, 1.0)
waiting_release_x = False
waiting_release_y = False
print("[BRIDGE] trigger loop ready — press R2+X to start, R2+Y to stop.",
flush=True)
while not stop_event.is_set():
time.sleep(period)
if not hub.first_state:
continue
r2x = hub.combo_r2x()
r2y = hub.combo_r2y()
if waiting_release_x:
if not r2x:
waiting_release_x = False
elif r2x:
waiting_release_x = True
print("[BRIDGE] R2+X pressed -> start saqr", flush=True)
try:
bridge.start_saqr()
except Exception as e:
print(f"[BRIDGE][ERR] start_saqr failed: {e}", flush=True)
if waiting_release_y:
if not r2y:
waiting_release_y = False
elif r2y:
waiting_release_y = True
print("[BRIDGE] R2+Y pressed -> stop saqr", flush=True)
try:
bridge.stop_saqr()
except Exception as e:
print(f"[BRIDGE][ERR] stop_saqr failed: {e}", flush=True)
def main():
bridge_argv, saqr_extra = split_argv(sys.argv[1:])
ap = argparse.ArgumentParser(
description="Bridge Saqr PPE events to the G1 arm 'reject' action."
)
ap.add_argument("--iface", default=None,
help="DDS network interface (e.g. eth0).")
ap.add_argument("--timeout", type=float, default=10.0)
ap.add_argument("--cooldown", type=float, default=8.0)
ap.add_argument("--release-after", type=float, default=2.0)
ap.add_argument("--dry-run", action="store_true")
ap.add_argument("--speaker-id", type=int, default=TTS_SPEAKER_ID)
ap.add_argument("--no-trigger", action="store_true")
ap.add_argument("--source", default=None)
ap.add_argument("--headless", action="store_true")
ap.add_argument("--saqr-conf", type=float, default=None)
ap.add_argument("--imgsz", type=int, default=None)
ap.add_argument("--device", default=None)
args = ap.parse_args(bridge_argv)
saqr_args: list = []
if args.source is not None:
saqr_args += ["--source", args.source]
if args.headless:
saqr_args += ["--headless"]
if args.saqr_conf is not None:
saqr_args += ["--conf", str(args.saqr_conf)]
if args.imgsz is not None:
saqr_args += ["--imgsz", str(args.imgsz)]
if args.device is not None:
saqr_args += ["--device", args.device]
saqr_args += saqr_extra
use_trigger = not args.no_trigger and not args.dry_run
robot = RobotController(
iface=args.iface,
timeout=args.timeout,
dry_run=args.dry_run,
tts_speaker_id=args.speaker_id,
want_lowstate=use_trigger,
)
env = os.environ.copy()
env["PYTHONUNBUFFERED"] = "1"
bridge = Bridge(
robot=robot,
cooldown_s=args.cooldown,
release_after_s=args.release_after,
saqr_args=saqr_args,
env=env,
cwd=str(PROJECT_ROOT),
)
print(f"[BRIDGE] saqr cmd template: {' '.join(build_saqr_cmd(saqr_args))}",
flush=True)
print(f"[BRIDGE] cwd: {PROJECT_ROOT}", flush=True)
stop_event = threading.Event()
def _forward_signal(signum, _frame):
print(f"[BRIDGE] signal {signum} -> shutting down", flush=True)
stop_event.set()
signal.signal(signal.SIGINT, _forward_signal)
signal.signal(signal.SIGTERM, _forward_signal)
have_hub = use_trigger and robot.hub is not None
if use_trigger and not have_hub:
print("[BRIDGE][WARN] --no-trigger not set, but no LowState hub is "
"available. Falling back to legacy auto-start mode.", flush=True)
trigger_thread: Optional[threading.Thread] = None
try:
if have_hub:
try:
robot.speak(TTS_BRIDGE_READY)
except Exception as e:
print(f"[BRIDGE][WARN] startup announce failed: {e}", flush=True)
trigger_thread = threading.Thread(
target=trigger_loop,
args=(bridge, robot.hub, stop_event),
daemon=True,
)
trigger_thread.start()
while not stop_event.is_set():
time.sleep(0.2)
else:
bridge.start_saqr()
while not stop_event.is_set() and bridge.is_running():
time.sleep(0.2)
finally:
if bridge.is_running():
bridge.stop_saqr()
stop_event.set()
if trigger_thread is not None:
trigger_thread.join(timeout=1.0)
try:
robot.shutdown_tts()
except Exception:
pass
print("[BRIDGE] bye.", flush=True)
sys.exit(0)
if __name__ == "__main__":
main()

0
controller.py → saqr/robot/controller.py
View File

216
saqr/robot/robot_controller.py Normal file
View File

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

0
saqr/utils/__init__.py Normal file
View File

11
logger.py → saqr/utils/logger.py
View File

@ -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/<category>/<name>.log."""
"""Return a cached logger that writes to runtime/logs/<category>/<name>.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)

727
saqr_g1_bridge.py
View File

@ -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<id>\d+)\s*\|\s*"
r"(?P<event>NEW|STATUS_CHANGE)\s*\|\s*"
r"(?P<status>SAFE|PARTIAL|UNSAFE)\s*\|\s*"
r"wearing:\s*(?P<wearing>[^|]*?)\s*\|\s*"
r"missing:\s*(?P<missing>[^|]*?)\s*\|\s*"
r"unknown:\s*(?P<unknown>.*?)\s*$"
)
def _parse_list_field(s: str) -> list:
"""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()

106
scripts/deploy.sh Executable file
View File

@ -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 <<INSTALL_EOF
set -e
source ~/miniconda3/etc/profile.d/conda.sh
conda activate ${ROBOT_ENV}
cd ${REMOTE_DIR}
pip install -q -e . 2>/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

87
scripts/run_local.sh Executable file
View File

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

102
scripts/run_robot.sh Executable file
View File

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

22
scripts/saqr-bridge.service Normal file
View File

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

65
scripts/start_saqr.sh Executable file
View File

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

186
start.md
View File

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

70
start_saqr.sh
View File

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