eric-rolph commited on
Commit ·
1f2ecf4
1
Parent(s): e90786b
Fix: Manually adding missing python source files
Browse files- pyproject.toml +25 -0
- reachy_mini_sentry/__init__.py +0 -0
- reachy_mini_sentry/__pycache__/main.cpython-312.pyc +0 -0
- reachy_mini_sentry/main.py +436 -0
- reachy_mini_sentry/patrol.py +97 -0
- reachy_mini_sentry/static/index.html +58 -0
- reachy_mini_sentry/static/style.css +105 -0
- reachy_mini_sentry/tracker.py +110 -0
pyproject.toml
ADDED
|
@@ -0,0 +1,25 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
[build-system]
|
| 2 |
+
requires = ["hatchling"]
|
| 3 |
+
build-backend = "hatchling.build"
|
| 4 |
+
|
| 5 |
+
[project]
|
| 6 |
+
name = "reachy-mini-sentry"
|
| 7 |
+
version = "0.1.2"
|
| 8 |
+
description = "Sentry Mode for Reachy Mini: Face tracking and intruder alerts!"
|
| 9 |
+
readme = "README.md"
|
| 10 |
+
requires-python = ">=3.12"
|
| 11 |
+
authors = [
|
| 12 |
+
{name = "Eric Rolph", email = "ericrolph@gmail.com"},
|
| 13 |
+
]
|
| 14 |
+
dependencies = [
|
| 15 |
+
"reachy-mini",
|
| 16 |
+
"opencv-python",
|
| 17 |
+
"mediapipe"
|
| 18 |
+
]
|
| 19 |
+
|
| 20 |
+
[project.entry-points.reachy_mini_apps]
|
| 21 |
+
reachy-mini-sentry = "reachy_mini_sentry.main:ReachyMiniSentry"
|
| 22 |
+
|
| 23 |
+
[tools.hatch.build.targets.wheel]
|
| 24 |
+
packages = ["reachy_mini_sentry"]
|
| 25 |
+
include = ["reachy_mini_sentry/sounds/*.wav", "reachy_mini_sentry/static/*"]
|
reachy_mini_sentry/__init__.py
ADDED
|
File without changes
|
reachy_mini_sentry/__pycache__/main.cpython-312.pyc
ADDED
|
Binary file (2.72 kB). View file
|
|
|
reachy_mini_sentry/main.py
ADDED
|
@@ -0,0 +1,436 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Guardian Laser Mode App for Reachy Mini.
|
| 2 |
+
|
| 3 |
+
A fun security application where Reachy acts as a guardian that:
|
| 4 |
+
- Smoothly patrols (scans) left-right when no threats detected
|
| 5 |
+
- When faces are spotted: zaps each one in sequence with laser!
|
| 6 |
+
- Choice of sounds via the web interface.
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
import threading
|
| 10 |
+
import time
|
| 11 |
+
import os
|
| 12 |
+
import io
|
| 13 |
+
import math
|
| 14 |
+
|
| 15 |
+
import cv2
|
| 16 |
+
from fastapi import FastAPI, Body
|
| 17 |
+
from fastapi.responses import StreamingResponse, HTMLResponse, JSONResponse
|
| 18 |
+
from fastapi.staticfiles import StaticFiles
|
| 19 |
+
|
| 20 |
+
from reachy_mini import ReachyMiniApp
|
| 21 |
+
from reachy_mini.reachy_mini import ReachyMini
|
| 22 |
+
from reachy_mini.utils import create_head_pose
|
| 23 |
+
|
| 24 |
+
from reachy_mini.utils import create_head_pose
|
| 25 |
+
from scipy.spatial.transform import Rotation as R
|
| 26 |
+
from scipy.spatial.transform import Rotation as Rot
|
| 27 |
+
|
| 28 |
+
try:
|
| 29 |
+
from reachy_mini_dances_library import DanceMove
|
| 30 |
+
DANCE_AVAILABLE = True
|
| 31 |
+
except ImportError:
|
| 32 |
+
DANCE_AVAILABLE = False
|
| 33 |
+
|
| 34 |
+
from .patrol import Patrol
|
| 35 |
+
from .tracker import FaceTracker
|
| 36 |
+
|
| 37 |
+
|
| 38 |
+
class ReachyMiniSentry(ReachyMiniApp):
|
| 39 |
+
custom_app_url = "http://0.0.0.0:8044"
|
| 40 |
+
dont_start_webserver = True
|
| 41 |
+
|
| 42 |
+
def __init__(self):
|
| 43 |
+
super().__init__()
|
| 44 |
+
self.latest_frame = None
|
| 45 |
+
self.frame_lock = threading.Lock()
|
| 46 |
+
self.detected_faces = []
|
| 47 |
+
self.is_running = False
|
| 48 |
+
|
| 49 |
+
# Sound configuration
|
| 50 |
+
self.sounds_dir = os.path.join(os.path.dirname(__file__), 'sounds')
|
| 51 |
+
self.available_sounds = {
|
| 52 |
+
"Alarm 1": {"file": "alarm_1.wav", "duration": None},
|
| 53 |
+
"Alarm 2": {"file": "alarm_2.wav", "duration": None},
|
| 54 |
+
"Alarm 3": {"file": "alarm_3.wav", "duration": None},
|
| 55 |
+
"Alarm 4": {"file": "alarm_4.wav", "duration": 4.2},
|
| 56 |
+
"Alarm 5": {"file": "alarm_5.wav", "duration": 4.0},
|
| 57 |
+
"Alarm 6": {"file": "alarm_6.wav", "duration": 4.0},
|
| 58 |
+
"Alarm 7": {"file": "alarm_7.wav", "duration": 14.0},
|
| 59 |
+
}
|
| 60 |
+
self.selected_sound_key = "Alarm 2"
|
| 61 |
+
|
| 62 |
+
# Smooth Tracking State (Degrees)
|
| 63 |
+
self.cur_yaw = 0.0
|
| 64 |
+
self.cur_pitch = 0.0
|
| 65 |
+
self.cur_body_yaw = 0.0
|
| 66 |
+
self.tracking_active = False
|
| 67 |
+
self.smooth_dx = 0.0
|
| 68 |
+
self.smooth_dy = 0.0
|
| 69 |
+
|
| 70 |
+
# Dancing State (Easter Egg for Alarm 7)
|
| 71 |
+
self.dancing = False
|
| 72 |
+
self.dance_start_time = 0.0
|
| 73 |
+
self.dance_move = None
|
| 74 |
+
self.dance_reachy = None
|
| 75 |
+
|
| 76 |
+
def run(self, reachy: ReachyMini, stop_event: threading.Event):
|
| 77 |
+
print("🚨 Sentry Mode Activated!")
|
| 78 |
+
|
| 79 |
+
# Start web server
|
| 80 |
+
self._start_web_server(stop_event)
|
| 81 |
+
|
| 82 |
+
patrol = Patrol(reachy)
|
| 83 |
+
tracker = FaceTracker()
|
| 84 |
+
|
| 85 |
+
faces_zapped_count = 0
|
| 86 |
+
faces_visible_last_frame = 0
|
| 87 |
+
last_face_seen_time = 0.0
|
| 88 |
+
|
| 89 |
+
# Tracking state
|
| 90 |
+
self.target_face_id = None
|
| 91 |
+
self.is_running = True
|
| 92 |
+
|
| 93 |
+
try:
|
| 94 |
+
while not stop_event.is_set():
|
| 95 |
+
frame = reachy.media.get_frame()
|
| 96 |
+
if frame is None:
|
| 97 |
+
time.sleep(0.1)
|
| 98 |
+
continue
|
| 99 |
+
|
| 100 |
+
h, w = frame.shape[:2]
|
| 101 |
+
all_faces = tracker.detect_all(frame)
|
| 102 |
+
num_faces = len(all_faces)
|
| 103 |
+
|
| 104 |
+
with self.frame_lock:
|
| 105 |
+
self.latest_frame = frame.copy()
|
| 106 |
+
self.detected_faces = list(all_faces)
|
| 107 |
+
|
| 108 |
+
if num_faces == 0:
|
| 109 |
+
# NO FACES - Smoothly drift back toward neutral or continue scan
|
| 110 |
+
time_since_last = time.time() - last_face_seen_time
|
| 111 |
+
|
| 112 |
+
if faces_visible_last_frame > 0 and time_since_last > 0.5:
|
| 113 |
+
print("👋 Target lost. Returning to idle...")
|
| 114 |
+
self.tracking_active = False
|
| 115 |
+
if faces_visible_last_frame > 0 and time_since_last > 0.5:
|
| 116 |
+
print(f"?? Lost visual. Holding position (Grace period)...")
|
| 117 |
+
self.tracking_active = False
|
| 118 |
+
faces_visible_last_frame = 0
|
| 119 |
+
|
| 120 |
+
# Only reset zaps and start patrol after a 2-second grace period
|
| 121 |
+
if time_since_last > 2.0:
|
| 122 |
+
# Grace period over - Relax back to center
|
| 123 |
+
self.cur_yaw *= 0.95
|
| 124 |
+
self.cur_pitch *= 0.95
|
| 125 |
+
self.cur_body_yaw *= 0.95
|
| 126 |
+
|
| 127 |
+
faces_zapped_count = 0
|
| 128 |
+
if abs(self.cur_yaw) < 5 and abs(self.cur_pitch) < 5:
|
| 129 |
+
patrol.step()
|
| 130 |
+
|
| 131 |
+
else:
|
| 132 |
+
# FACES DETECTED!
|
| 133 |
+
last_face_seen_time = time.time()
|
| 134 |
+
self.tracking_active = True
|
| 135 |
+
primary_face = all_faces[0]
|
| 136 |
+
|
| 137 |
+
# Use look_at_image to compute the TARGET POSE (like conversation app!)
|
| 138 |
+
x, y, w_face, h_face = primary_face
|
| 139 |
+
# Face center in pixel coordinates
|
| 140 |
+
face_cx = x + w_face / 2
|
| 141 |
+
face_cy = y + h_face / 2
|
| 142 |
+
|
| 143 |
+
move_duration = 0.0 # Use 0 for immediate updates (set_target) to avoid stop-start jerkiness
|
| 144 |
+
|
| 145 |
+
# Visual Servoing: Use pixel error to drive the head directly.
|
| 146 |
+
try:
|
| 147 |
+
raw_x_err, raw_y_err = tracker.get_face_center(primary_face, w, h)
|
| 148 |
+
|
| 149 |
+
# Apply Smoothing
|
| 150 |
+
alpha = 0.5
|
| 151 |
+
if not hasattr(self, 'smooth_x_err'):
|
| 152 |
+
self.smooth_x_err = raw_x_err
|
| 153 |
+
self.smooth_y_err = raw_y_err
|
| 154 |
+
else:
|
| 155 |
+
self.smooth_x_err = alpha * raw_x_err + (1 - alpha) * self.smooth_x_err
|
| 156 |
+
self.smooth_y_err = alpha * raw_y_err + (1 - alpha) * self.smooth_y_err
|
| 157 |
+
|
| 158 |
+
# Target Offset: Target VERY HIGH (-0.6).
|
| 159 |
+
# User wants face near the top of the frame.
|
| 160 |
+
TARGET_Y_OFFSET = -0.6
|
| 161 |
+
|
| 162 |
+
eff_x_err = self.smooth_x_err
|
| 163 |
+
eff_y_err = self.smooth_y_err - TARGET_Y_OFFSET
|
| 164 |
+
|
| 165 |
+
# Gains (Aggressive)
|
| 166 |
+
K_YAW = 5.0
|
| 167 |
+
K_PITCH = 5.0
|
| 168 |
+
|
| 169 |
+
# Update state (Standard Logic: Positive Error means face low -> Look Down -> Add Pitch)
|
| 170 |
+
self.cur_yaw -= K_YAW * eff_x_err
|
| 171 |
+
self.cur_pitch += K_PITCH * eff_y_err
|
| 172 |
+
|
| 173 |
+
# Debug Print
|
| 174 |
+
print(f"TRACK: y_err={raw_y_err:.2f}, eff={eff_y_err:.2f}, pitch={self.cur_pitch:.1f}")
|
| 175 |
+
|
| 176 |
+
except Exception as e:
|
| 177 |
+
print(f"TRACKING ERROR: {e}")
|
| 178 |
+
|
| 179 |
+
# Hardware Safety Limits - Expanded to 60 degrees (some robots can go further)
|
| 180 |
+
self.cur_pitch = max(min(self.cur_pitch, 60), -60)
|
| 181 |
+
self.cur_yaw = max(min(self.cur_yaw, 55), -55)
|
| 182 |
+
self.cur_body_yaw = max(min(self.cur_body_yaw, 45), -45)
|
| 183 |
+
|
| 184 |
+
# Simple antenna pointing
|
| 185 |
+
antenna_goal = [0.0, 0.0]
|
| 186 |
+
if num_faces > faces_zapped_count:
|
| 187 |
+
print(f"🎯 ALERT! Targeting new face #{faces_zapped_count + 1}")
|
| 188 |
+
|
| 189 |
+
# Trigger alarm sound
|
| 190 |
+
sound_info = self.available_sounds.get(self.selected_sound_key)
|
| 191 |
+
if sound_info:
|
| 192 |
+
sound_path = os.path.join(self.sounds_dir, sound_info["file"])
|
| 193 |
+
if os.path.exists(sound_path):
|
| 194 |
+
reachy.media.play_sound(sound_path)
|
| 195 |
+
|
| 196 |
+
# Easter Egg: Alarm 7 triggers dance mode!
|
| 197 |
+
if self.selected_sound_key == "Alarm 7":
|
| 198 |
+
self.dancing = True
|
| 199 |
+
self.dance_start_time = time.time()
|
| 200 |
+
print(f"🕺🕺🕺 DANCE MODE ACTIVATED! Time: {self.dance_start_time} 🕺🕺🕺")
|
| 201 |
+
else:
|
| 202 |
+
print(f"Playing sound: {self.selected_sound_key}")
|
| 203 |
+
|
| 204 |
+
faces_zapped_count = num_faces
|
| 205 |
+
|
| 206 |
+
# 7. Execute Movement (Or Dance!)
|
| 207 |
+
|
| 208 |
+
if self.dancing:
|
| 209 |
+
# DANCE MODE: Override EVERYTHING.
|
| 210 |
+
# No tracking, no safety limits (we set safe dance moves), no fighting.
|
| 211 |
+
dance_elapsed = time.time() - self.dance_start_time
|
| 212 |
+
|
| 213 |
+
if dance_elapsed > 14.0:
|
| 214 |
+
self.dancing = False # Party's over
|
| 215 |
+
print("💃 Dance complete! Resuming Sentry Mode.")
|
| 216 |
+
# Reset to safe pose
|
| 217 |
+
self.cur_yaw = 0.0
|
| 218 |
+
self.cur_pitch = 0.0
|
| 219 |
+
|
| 220 |
+
else:
|
| 221 |
+
# 100 BPM = 1.666 Hz.
|
| 222 |
+
# We want a continuous, energetic headbob.
|
| 223 |
+
# Pitch: Center (0) to Down (25) and Up (-10)? Or just Up/Down around 0?
|
| 224 |
+
# "Up and down movement". Let's go +/- 20 degrees.
|
| 225 |
+
# sin(2*pi*f*t) -> -1 to 1.
|
| 226 |
+
|
| 227 |
+
beat_freq = 100.0 / 60.0 # ~1.67 Hz
|
| 228 |
+
|
| 229 |
+
# Phase shift to start going DOWN first (usually on beat 1)
|
| 230 |
+
# sin starts 0 -> up (negative pitch).
|
| 231 |
+
# We want to go DOWN (positive pitch) first? Or down on the beat?
|
| 232 |
+
# Let's just use cos. cos(0)=1 (Down 20), cos(pi)=-1 (Up 20).
|
| 233 |
+
bob = math.cos(2 * math.pi * beat_freq * dance_elapsed)
|
| 234 |
+
|
| 235 |
+
# Map to Pitch: +/- 20 degrees
|
| 236 |
+
target_pitch = 20.0 * bob
|
| 237 |
+
|
| 238 |
+
# Yaw: Keep centered or sway slightly?
|
| 239 |
+
# User said "continuous headbob up and down".
|
| 240 |
+
# Let's keep Yaw fixed at 0 for pure bob.
|
| 241 |
+
target_yaw = 0.0
|
| 242 |
+
target_body_yaw = 0.0
|
| 243 |
+
|
| 244 |
+
# Antennas: Wave
|
| 245 |
+
# sin wave offset
|
| 246 |
+
wave = math.sin(2 * math.pi * beat_freq * dance_elapsed)
|
| 247 |
+
target_antennas = [0.6 * wave, -0.6 * wave]
|
| 248 |
+
|
| 249 |
+
reachy.goto_target(
|
| 250 |
+
head=create_head_pose(yaw=target_yaw, pitch=target_pitch, degrees=True, mm=True),
|
| 251 |
+
body_yaw=math.radians(target_body_yaw),
|
| 252 |
+
antennas=target_antennas,
|
| 253 |
+
duration=0.0 # Instant update
|
| 254 |
+
)
|
| 255 |
+
|
| 256 |
+
# Skip the standard goto_target below
|
| 257 |
+
continue
|
| 258 |
+
|
| 259 |
+
# Use set_target for smoother, non-blocking updates (requires duration=0 in goto or direct call)
|
| 260 |
+
# goto_target with duration=0 effectively calls set_target
|
| 261 |
+
reachy.goto_target(
|
| 262 |
+
head=create_head_pose(yaw=self.cur_yaw, pitch=self.cur_pitch, degrees=True, mm=True),
|
| 263 |
+
body_yaw=float(math.radians(self.cur_body_yaw)),
|
| 264 |
+
antennas=antenna_goal,
|
| 265 |
+
duration=0.0 # Instant update
|
| 266 |
+
)
|
| 267 |
+
|
| 268 |
+
faces_visible_last_frame = num_faces
|
| 269 |
+
|
| 270 |
+
# 30Hz loop (to match camera/interpolation rate)
|
| 271 |
+
time.sleep(0.03)
|
| 272 |
+
|
| 273 |
+
except Exception as e:
|
| 274 |
+
print(f"Guardian Error: {e}")
|
| 275 |
+
finally:
|
| 276 |
+
self.is_running = False
|
| 277 |
+
tracker.close()
|
| 278 |
+
print("🔫 Guardian Laser Mode Deactivated.")
|
| 279 |
+
|
| 280 |
+
def _run_dance_routine(self, reachy):
|
| 281 |
+
"""Execute dance moves from the dance library synced to 100 BPM music."""
|
| 282 |
+
try:
|
| 283 |
+
if not DANCE_AVAILABLE:
|
| 284 |
+
return
|
| 285 |
+
|
| 286 |
+
print("🕺 DANCE MODE ACTIVATED!")
|
| 287 |
+
|
| 288 |
+
# Wait for the 2-second intro ("IN-TRU-DER. Alert!")
|
| 289 |
+
time.sleep(2.0)
|
| 290 |
+
|
| 291 |
+
# Now the music starts - 12 seconds of 100 BPM beats
|
| 292 |
+
# We'll cycle through different dance moves
|
| 293 |
+
|
| 294 |
+
# Dance sequence: 4 moves, 3 seconds each = 12 seconds total
|
| 295 |
+
dance_moves = [
|
| 296 |
+
("headbanger_combo", 3.0),
|
| 297 |
+
("chicken_peck", 3.0),
|
| 298 |
+
("groovy_sway_and_roll", 3.0),
|
| 299 |
+
("headbanger_combo", 3.0),
|
| 300 |
+
]
|
| 301 |
+
|
| 302 |
+
for move_name, duration in dance_moves:
|
| 303 |
+
if not self.dancing:
|
| 304 |
+
break
|
| 305 |
+
try:
|
| 306 |
+
# Create dance move with 100 BPM
|
| 307 |
+
move = DanceMove(move_name)
|
| 308 |
+
move.default_bpm = 100.0
|
| 309 |
+
# Play for the specified duration
|
| 310 |
+
move.play_on(reachy, repeat=int(duration * (100.0/60.0)))
|
| 311 |
+
except Exception as e:
|
| 312 |
+
print(f"Dance move '{move_name}' failed: {e}")
|
| 313 |
+
time.sleep(duration)
|
| 314 |
+
|
| 315 |
+
print("💃 Dance routine complete!")
|
| 316 |
+
self.dancing = False
|
| 317 |
+
|
| 318 |
+
except Exception as e:
|
| 319 |
+
print(f"Dance routine error: {e}")
|
| 320 |
+
self.dancing = False
|
| 321 |
+
|
| 322 |
+
def _start_web_server(self, stop_event):
|
| 323 |
+
import uvicorn
|
| 324 |
+
from urllib.parse import urlparse
|
| 325 |
+
|
| 326 |
+
app = FastAPI()
|
| 327 |
+
|
| 328 |
+
@app.get("/")
|
| 329 |
+
async def index():
|
| 330 |
+
sound_options = "".join([f'<option value="{k}" {"selected" if k == self.selected_sound_key else ""}>{k}</option>'
|
| 331 |
+
for k in self.available_sounds.keys()])
|
| 332 |
+
html = f"""
|
| 333 |
+
<!DOCTYPE html>
|
| 334 |
+
<html>
|
| 335 |
+
<head>
|
| 336 |
+
<title>🚨 Sentry Mode</title>
|
| 337 |
+
<style>
|
| 338 |
+
body {{ font-family: 'Segoe UI', Arial, sans-serif; background: #1a1a2e; color: #eee; margin: 0; padding: 20px; text-align: center; }}
|
| 339 |
+
.container {{ max-width: 800px; margin: 0 auto; }}
|
| 340 |
+
h1 {{ color: #70a1ff; text-shadow: 0 0 20px rgba(112,161,255,0.5); }}
|
| 341 |
+
.video-container {{ background: #000; border-radius: 15px; overflow: hidden; margin-bottom: 20px; border: 3px solid #333; }}
|
| 342 |
+
.video-container img {{ width: 100%; display: block; }}
|
| 343 |
+
.controls {{ background: rgba(255,255,255,0.05); padding: 20px; border-radius: 10px; margin-top: 20px; }}
|
| 344 |
+
select {{ background: #16213e; color: white; padding: 10px; border-radius: 5px; border: 1px solid #70a1ff; width: 100%; max-width: 300px; font-size: 1em; }}
|
| 345 |
+
label {{ display: block; margin-bottom: 10px; color: #70a1ff; font-weight: bold; }}
|
| 346 |
+
</style>
|
| 347 |
+
</head>
|
| 348 |
+
<body>
|
| 349 |
+
<div class="container">
|
| 350 |
+
<h1>🚨 Sentry Mode</h1>
|
| 351 |
+
<div class="video-container">
|
| 352 |
+
<img src="/video" alt="Live Feed">
|
| 353 |
+
</div>
|
| 354 |
+
<div class="controls">
|
| 355 |
+
<label for="sound-select">Choose Alarm Sound:</label>
|
| 356 |
+
<select id="sound-select" onchange="updateSound(this.value)">
|
| 357 |
+
{sound_options}
|
| 358 |
+
</select>
|
| 359 |
+
</div>
|
| 360 |
+
</div>
|
| 361 |
+
<script>
|
| 362 |
+
function updateSound(val) {{
|
| 363 |
+
fetch('/set_sound', {{
|
| 364 |
+
method: 'POST',
|
| 365 |
+
headers: {{ 'Content-Type': 'application/json' }},
|
| 366 |
+
body: JSON.stringify({{ sound: val }})
|
| 367 |
+
}});
|
| 368 |
+
}}
|
| 369 |
+
</script>
|
| 370 |
+
</body>
|
| 371 |
+
</html>
|
| 372 |
+
"""
|
| 373 |
+
return HTMLResponse(content=html)
|
| 374 |
+
|
| 375 |
+
@app.get("/video")
|
| 376 |
+
async def video_feed():
|
| 377 |
+
def generate():
|
| 378 |
+
while self.is_running:
|
| 379 |
+
with self.frame_lock:
|
| 380 |
+
if self.latest_frame is None:
|
| 381 |
+
time.sleep(0.05)
|
| 382 |
+
continue
|
| 383 |
+
frame = self.latest_frame.copy()
|
| 384 |
+
faces = list(self.detected_faces)
|
| 385 |
+
for i, (x, y, w, h) in enumerate(faces):
|
| 386 |
+
cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 0, 255), 3)
|
| 387 |
+
cx, cy = x + w//2, y + h//2
|
| 388 |
+
cv2.circle(frame, (cx, cy), 30, (0, 255, 0), 2)
|
| 389 |
+
cv2.putText(frame, f"TARGET #{i+1}", (x, y-10), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|
| 390 |
+
|
| 391 |
+
# DEBUG OVERLAY: Show Tracking State
|
| 392 |
+
# This helps verify if the robot is *trying* to move and hitting limits, or moving wrong way.
|
| 393 |
+
if self.tracking_active:
|
| 394 |
+
debug_text = f"PITCH: {self.cur_pitch:.1f} | YAW: {self.cur_yaw:.1f}"
|
| 395 |
+
# If detecting, show error
|
| 396 |
+
if len(self.detected_faces) > 0:
|
| 397 |
+
# Re-calculate error for display (approximation)
|
| 398 |
+
# We don't have safe access to 'tracker' here easily without passing it or globals
|
| 399 |
+
# But we can assume detecting logic works.
|
| 400 |
+
cv2.putText(frame, debug_text, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2)
|
| 401 |
+
else:
|
| 402 |
+
cv2.putText(frame, f"{debug_text} (Idle)", (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (200, 200, 200), 2)
|
| 403 |
+
|
| 404 |
+
_, jpeg = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 70])
|
| 405 |
+
yield (b'--frame\r\nContent-Type: image/jpeg\r\n\r\n' + jpeg.tobytes() + b'\r\n')
|
| 406 |
+
time.sleep(0.05)
|
| 407 |
+
return StreamingResponse(generate(), media_type='multipart/x-mixed-replace; boundary=frame')
|
| 408 |
+
|
| 409 |
+
@app.post("/set_sound")
|
| 410 |
+
async def set_sound(data: dict = Body(...)):
|
| 411 |
+
new_sound = data.get("sound")
|
| 412 |
+
if new_sound in self.available_sounds:
|
| 413 |
+
self.selected_sound_key = new_sound
|
| 414 |
+
print(f"🔊 Sound changed to: {new_sound}")
|
| 415 |
+
return {{"status": "ok"}}
|
| 416 |
+
return JSONResponse(status_code=400, content={{"error": "Invalid sound"}})
|
| 417 |
+
|
| 418 |
+
url = urlparse(self.custom_app_url)
|
| 419 |
+
config = uvicorn.Config(app, host=url.hostname, port=url.port, log_level="warning")
|
| 420 |
+
server = uvicorn.Server(config)
|
| 421 |
+
|
| 422 |
+
def run_server():
|
| 423 |
+
import asyncio
|
| 424 |
+
loop = asyncio.new_event_loop()
|
| 425 |
+
asyncio.set_event_loop(loop)
|
| 426 |
+
loop.run_until_complete(server.serve())
|
| 427 |
+
|
| 428 |
+
threading.Thread(target=run_server, daemon=True).start()
|
| 429 |
+
|
| 430 |
+
|
| 431 |
+
if __name__ == "__main__":
|
| 432 |
+
app = ReachyMiniSentry()
|
| 433 |
+
try:
|
| 434 |
+
app.wrapped_run()
|
| 435 |
+
except KeyboardInterrupt:
|
| 436 |
+
app.stop()
|
reachy_mini_sentry/patrol.py
ADDED
|
@@ -0,0 +1,97 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import math
|
| 2 |
+
import time
|
| 3 |
+
import random
|
| 4 |
+
from reachy_mini.utils import create_head_pose
|
| 5 |
+
|
| 6 |
+
|
| 7 |
+
class Patrol:
|
| 8 |
+
"""Human-realistic organic patrol/scanning behavior.
|
| 9 |
+
|
| 10 |
+
Instead of a mechanical sine wave, this mimics human behavior:
|
| 11 |
+
1. Pick a random point of interest.
|
| 12 |
+
2. Move there smoothly.
|
| 13 |
+
3. Pause and 'scan' (micro-movements).
|
| 14 |
+
4. Repeat.
|
| 15 |
+
"""
|
| 16 |
+
|
| 17 |
+
def __init__(self, reachy):
|
| 18 |
+
self.reachy = reachy
|
| 19 |
+
self.last_move_finish_time = 0
|
| 20 |
+
self.current_state = "IDLE" # IDLE, MOVING, SCANNING
|
| 21 |
+
self.next_action_time = 0
|
| 22 |
+
|
| 23 |
+
# State targets
|
| 24 |
+
self.target_head_yaw = 0
|
| 25 |
+
self.target_body_yaw = 0
|
| 26 |
+
self.target_pitch = 0
|
| 27 |
+
|
| 28 |
+
def reset(self):
|
| 29 |
+
"""Reset patrol state."""
|
| 30 |
+
self.current_state = "IDLE"
|
| 31 |
+
self.next_action_time = time.time()
|
| 32 |
+
|
| 33 |
+
def step(self):
|
| 34 |
+
"""Update patrol behavior."""
|
| 35 |
+
now = time.time()
|
| 36 |
+
|
| 37 |
+
# If we are waiting for the next action...
|
| 38 |
+
if now < self.next_action_time:
|
| 39 |
+
return
|
| 40 |
+
|
| 41 |
+
# DECISION LOGIC
|
| 42 |
+
if self.current_state == "IDLE" or self.current_state == "SCANNING":
|
| 43 |
+
# Pick a new point of interest to look at
|
| 44 |
+
self.current_state = "MOVING"
|
| 45 |
+
|
| 46 |
+
# WIDE SCAN: Maximize Head + Body limits
|
| 47 |
+
# Head: +/- 55, Body: +/- 45. Total: +/- 100 degrees from center.
|
| 48 |
+
# We want to use the full range to simulate "300 degree scan" (or as close as possible).
|
| 49 |
+
|
| 50 |
+
# Pick a random "Sector" to ensure we look around widely
|
| 51 |
+
# 0=Center, 1=Far Left, 2=Far Right, 3=Random Up/Down
|
| 52 |
+
sector = random.choice([0, 1, 1, 2, 2, 3])
|
| 53 |
+
|
| 54 |
+
if sector == 0: # Centerish
|
| 55 |
+
self.target_head_yaw = random.uniform(-20, 20)
|
| 56 |
+
self.target_body_yaw = random.uniform(-10, 10)
|
| 57 |
+
self.target_pitch = random.uniform(-10, 15)
|
| 58 |
+
move_duration = random.uniform(1.5, 2.5)
|
| 59 |
+
elif sector == 1: # Far Left
|
| 60 |
+
self.target_head_yaw = random.uniform(30, 50) # Positive is Left
|
| 61 |
+
self.target_body_yaw = random.uniform(20, 40)
|
| 62 |
+
self.target_pitch = random.uniform(-10, 10) # Look up/down
|
| 63 |
+
move_duration = random.uniform(2.0, 3.5)
|
| 64 |
+
elif sector == 2: # Far Right
|
| 65 |
+
self.target_head_yaw = random.uniform(-50, -30) # Negative is Right
|
| 66 |
+
self.target_body_yaw = random.uniform(-40, -20)
|
| 67 |
+
self.target_pitch = random.uniform(-10, 10)
|
| 68 |
+
move_duration = random.uniform(2.0, 3.5)
|
| 69 |
+
else: # Random Up/Down Check (Ceiling/Floor)
|
| 70 |
+
self.target_head_yaw = random.uniform(-30, 30)
|
| 71 |
+
self.target_body_yaw = random.uniform(-20, 20)
|
| 72 |
+
self.target_pitch = random.choice([random.uniform(15, 25), random.uniform(-20, -10)]) # Distinct look up or down
|
| 73 |
+
move_duration = random.uniform(1.5, 2.5)
|
| 74 |
+
|
| 75 |
+
# Execution
|
| 76 |
+
target_pose = create_head_pose(
|
| 77 |
+
yaw=self.target_head_yaw,
|
| 78 |
+
pitch=self.target_pitch,
|
| 79 |
+
degrees=True, mm=True
|
| 80 |
+
)
|
| 81 |
+
|
| 82 |
+
# Antenna "Interest" - Perking up
|
| 83 |
+
ant_pos = [random.uniform(-0.5, 0.5), random.uniform(-0.5, 0.5)]
|
| 84 |
+
|
| 85 |
+
self.reachy.goto_target(
|
| 86 |
+
head=target_pose,
|
| 87 |
+
body_yaw=math.radians(self.target_body_yaw),
|
| 88 |
+
antennas=ant_pos,
|
| 89 |
+
duration=move_duration
|
| 90 |
+
)
|
| 91 |
+
|
| 92 |
+
# Next state: Pause and stare for a bit
|
| 93 |
+
self.current_state = "SCANNING"
|
| 94 |
+
# Wait for move to finish + a pause time (0.5s to 2.0s)
|
| 95 |
+
self.next_action_time = now + move_duration + random.uniform(0.5, 2.0)
|
| 96 |
+
|
| 97 |
+
print(f"👀 Patrol: Looking at sector {sector} (H:{self.target_head_yaw:.0f}, B:{self.target_body_yaw:.0f}, P:{self.target_pitch:.0f})")
|
reachy_mini_sentry/static/index.html
ADDED
|
@@ -0,0 +1,58 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
<!DOCTYPE html>
|
| 2 |
+
<html lang="en">
|
| 3 |
+
|
| 4 |
+
<head>
|
| 5 |
+
<meta charset="UTF-8">
|
| 6 |
+
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
| 7 |
+
<title>🚨 Reachy Sentry</title>
|
| 8 |
+
<link rel="stylesheet" href="style.css">
|
| 9 |
+
<!-- Use Google Fonts for a premium feel -->
|
| 10 |
+
<link href="https://fonts.googleapis.com/css2?family=Outfit:wght@300;500;700&display=swap" rel="stylesheet">
|
| 11 |
+
</head>
|
| 12 |
+
|
| 13 |
+
<body>
|
| 14 |
+
<div class="container sentry-container">
|
| 15 |
+
<header>
|
| 16 |
+
<h1>Sentry Mode</h1>
|
| 17 |
+
<p id="status-text">System is Active</p>
|
| 18 |
+
</header>
|
| 19 |
+
|
| 20 |
+
<div class="status-display">
|
| 21 |
+
<div class="radar">
|
| 22 |
+
<div class="sweep"></div>
|
| 23 |
+
</div>
|
| 24 |
+
<p id="log">Scanning Area...</p>
|
| 25 |
+
</div>
|
| 26 |
+
|
| 27 |
+
<div class="controls">
|
| 28 |
+
<button id="btn-toggle" class="btn primary" onclick="toggleSentry()">Deactivate System</button>
|
| 29 |
+
</div>
|
| 30 |
+
</div>
|
| 31 |
+
|
| 32 |
+
<script>
|
| 33 |
+
let isActive = true;
|
| 34 |
+
|
| 35 |
+
function toggleSentry() {
|
| 36 |
+
isActive = !isActive;
|
| 37 |
+
const btn = document.getElementById('btn-toggle');
|
| 38 |
+
const status = document.getElementById('status-text');
|
| 39 |
+
const log = document.getElementById('log');
|
| 40 |
+
|
| 41 |
+
if (isActive) {
|
| 42 |
+
document.body.className = 'mode-active';
|
| 43 |
+
btn.textContent = "Deactivate System";
|
| 44 |
+
btn.className = "btn primary";
|
| 45 |
+
status.textContent = "System is Active";
|
| 46 |
+
log.textContent = "Scanning Area...";
|
| 47 |
+
} else {
|
| 48 |
+
document.body.className = 'mode-inactive';
|
| 49 |
+
btn.textContent = "Activate System";
|
| 50 |
+
btn.className = "btn secondary";
|
| 51 |
+
status.textContent = "System is Offline";
|
| 52 |
+
log.textContent = "Standby";
|
| 53 |
+
}
|
| 54 |
+
}
|
| 55 |
+
</script>
|
| 56 |
+
</body>
|
| 57 |
+
|
| 58 |
+
</html>
|
reachy_mini_sentry/static/style.css
ADDED
|
@@ -0,0 +1,105 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
:root {
|
| 2 |
+
--bg-color: #0f172a;
|
| 3 |
+
--text-color: #ffffff;
|
| 4 |
+
--primary-color: #ef4444;
|
| 5 |
+
/* Red 500 */
|
| 6 |
+
--secondary-color: #3b82f6;
|
| 7 |
+
/* Blue 500 */
|
| 8 |
+
--font-main: 'Outfit', sans-serif;
|
| 9 |
+
}
|
| 10 |
+
|
| 11 |
+
body {
|
| 12 |
+
background-color: #111;
|
| 13 |
+
color: var(--text-color);
|
| 14 |
+
font-family: var(--font-main);
|
| 15 |
+
display: flex;
|
| 16 |
+
justify-content: center;
|
| 17 |
+
align-items: center;
|
| 18 |
+
height: 100vh;
|
| 19 |
+
margin: 0;
|
| 20 |
+
transition: background-color 0.5s;
|
| 21 |
+
}
|
| 22 |
+
|
| 23 |
+
body.mode-active {
|
| 24 |
+
background: radial-gradient(circle, #330000 0%, #000000 100%);
|
| 25 |
+
}
|
| 26 |
+
|
| 27 |
+
body.mode-inactive {
|
| 28 |
+
background-color: #0f172a;
|
| 29 |
+
}
|
| 30 |
+
|
| 31 |
+
.container {
|
| 32 |
+
background: rgba(20, 20, 20, 0.8);
|
| 33 |
+
backdrop-filter: blur(10px);
|
| 34 |
+
padding: 3rem;
|
| 35 |
+
border-radius: 12px;
|
| 36 |
+
box-shadow: 0 0 50px rgba(255, 0, 0, 0.2);
|
| 37 |
+
text-align: center;
|
| 38 |
+
width: 350px;
|
| 39 |
+
border: 1px solid rgba(255, 0, 0, 0.3);
|
| 40 |
+
}
|
| 41 |
+
|
| 42 |
+
h1 {
|
| 43 |
+
font-weight: 700;
|
| 44 |
+
margin-bottom: 0.5rem;
|
| 45 |
+
text-transform: uppercase;
|
| 46 |
+
letter-spacing: 2px;
|
| 47 |
+
}
|
| 48 |
+
|
| 49 |
+
.radar {
|
| 50 |
+
width: 150px;
|
| 51 |
+
height: 150px;
|
| 52 |
+
border: 2px solid #ef4444;
|
| 53 |
+
border-radius: 50%;
|
| 54 |
+
margin: 2rem auto;
|
| 55 |
+
position: relative;
|
| 56 |
+
background:
|
| 57 |
+
radial-gradient(circle, transparent 20%, #ef4444 20%, #ef4444 21%, transparent 21%),
|
| 58 |
+
radial-gradient(circle, transparent 50%, #ef4444 50%, #ef4444 51%, transparent 51%),
|
| 59 |
+
radial-gradient(circle, transparent 80%, #ef4444 80%, #ef4444 81%, transparent 81%);
|
| 60 |
+
box-shadow: 0 0 20px rgba(239, 68, 68, 0.5);
|
| 61 |
+
}
|
| 62 |
+
|
| 63 |
+
.sweep {
|
| 64 |
+
position: absolute;
|
| 65 |
+
top: 50%;
|
| 66 |
+
left: 50%;
|
| 67 |
+
width: 50%;
|
| 68 |
+
height: 2px;
|
| 69 |
+
background: #ef4444;
|
| 70 |
+
transform-origin: 0 0;
|
| 71 |
+
animation: sweep 2s infinite linear;
|
| 72 |
+
box-shadow: 0 0 10px #ef4444;
|
| 73 |
+
}
|
| 74 |
+
|
| 75 |
+
@keyframes sweep {
|
| 76 |
+
from {
|
| 77 |
+
transform: rotate(0deg);
|
| 78 |
+
}
|
| 79 |
+
|
| 80 |
+
to {
|
| 81 |
+
transform: rotate(360deg);
|
| 82 |
+
}
|
| 83 |
+
}
|
| 84 |
+
|
| 85 |
+
.btn {
|
| 86 |
+
border: none;
|
| 87 |
+
padding: 1rem 2rem;
|
| 88 |
+
border-radius: 4px;
|
| 89 |
+
font-weight: 700;
|
| 90 |
+
text-transform: uppercase;
|
| 91 |
+
cursor: pointer;
|
| 92 |
+
font-family: var(--font-main);
|
| 93 |
+
letter-spacing: 1px;
|
| 94 |
+
}
|
| 95 |
+
|
| 96 |
+
.btn.primary {
|
| 97 |
+
background-color: var(--primary-color);
|
| 98 |
+
color: white;
|
| 99 |
+
box-shadow: 0 0 20px rgba(239, 68, 68, 0.4);
|
| 100 |
+
}
|
| 101 |
+
|
| 102 |
+
.btn.secondary {
|
| 103 |
+
background-color: var(--secondary-color);
|
| 104 |
+
color: white;
|
| 105 |
+
}
|
reachy_mini_sentry/tracker.py
ADDED
|
@@ -0,0 +1,110 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""Face detection using MediaPipe for Sentry Mode.
|
| 2 |
+
|
| 3 |
+
MediaPipe is much more accurate than Haar Cascades:
|
| 4 |
+
- Better at detecting faces at angles and distances
|
| 5 |
+
- Fewer false positives
|
| 6 |
+
- Optimized for ARM/edge devices
|
| 7 |
+
"""
|
| 8 |
+
|
| 9 |
+
import cv2
|
| 10 |
+
import mediapipe as mp
|
| 11 |
+
|
| 12 |
+
|
| 13 |
+
class FaceTracker:
|
| 14 |
+
"""Detects faces using MediaPipe Face Detection."""
|
| 15 |
+
|
| 16 |
+
def __init__(self):
|
| 17 |
+
# Initialize MediaPipe Face Detection
|
| 18 |
+
self.mp_face = mp.solutions.face_detection
|
| 19 |
+
|
| 20 |
+
# model_selection=1 is for full-range detection (better at distance)
|
| 21 |
+
# min_detection_confidence=0.75 for fewer false positives (ghosts)
|
| 22 |
+
self.detector = self.mp_face.FaceDetection(
|
| 23 |
+
model_selection=1, # 0=within 2m, 1=full range (up to 5m)
|
| 24 |
+
min_detection_confidence=0.75
|
| 25 |
+
)
|
| 26 |
+
|
| 27 |
+
print("✅ MediaPipe (Google) Face Detection initialized (High Confidence Mode)")
|
| 28 |
+
|
| 29 |
+
def detect_all(self, frame):
|
| 30 |
+
"""Detect ALL faces in the frame using MediaPipe.
|
| 31 |
+
|
| 32 |
+
Returns:
|
| 33 |
+
List of face bounding boxes [(x, y, w, h), ...] sorted by size (largest first)
|
| 34 |
+
Empty list if no faces detected
|
| 35 |
+
"""
|
| 36 |
+
try:
|
| 37 |
+
# Convert BGR to RGB (MediaPipe expects RGB)
|
| 38 |
+
rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
| 39 |
+
|
| 40 |
+
# Run detection
|
| 41 |
+
results = self.detector.process(rgb_frame)
|
| 42 |
+
|
| 43 |
+
if not results.detections:
|
| 44 |
+
return []
|
| 45 |
+
|
| 46 |
+
h, w = frame.shape[:2]
|
| 47 |
+
faces = []
|
| 48 |
+
|
| 49 |
+
for detection in results.detections:
|
| 50 |
+
# Get bounding box (relative coordinates)
|
| 51 |
+
bbox = detection.location_data.relative_bounding_box
|
| 52 |
+
|
| 53 |
+
# Convert to pixel coordinates
|
| 54 |
+
x = int(bbox.xmin * w)
|
| 55 |
+
y = int(bbox.ymin * h)
|
| 56 |
+
fw = int(bbox.width * w)
|
| 57 |
+
fh = int(bbox.height * h)
|
| 58 |
+
|
| 59 |
+
# Clamp to frame bounds
|
| 60 |
+
x = max(0, x)
|
| 61 |
+
y = max(0, y)
|
| 62 |
+
fw = min(fw, w - x)
|
| 63 |
+
fh = min(fh, h - y)
|
| 64 |
+
|
| 65 |
+
if fw > 0 and fh > 0:
|
| 66 |
+
faces.append((x, y, fw, fh))
|
| 67 |
+
|
| 68 |
+
# Sort by face area (largest first = closest person)
|
| 69 |
+
faces.sort(key=lambda f: f[2] * f[3], reverse=True)
|
| 70 |
+
|
| 71 |
+
return faces
|
| 72 |
+
|
| 73 |
+
except Exception as e:
|
| 74 |
+
print(f"Detection Error: {e}")
|
| 75 |
+
return []
|
| 76 |
+
|
| 77 |
+
def detect(self, frame):
|
| 78 |
+
"""Detect single largest face (backward compatible).
|
| 79 |
+
|
| 80 |
+
Returns:
|
| 81 |
+
Largest face bounding box (x, y, w, h) or None
|
| 82 |
+
"""
|
| 83 |
+
faces = self.detect_all(frame)
|
| 84 |
+
return faces[0] if faces else None
|
| 85 |
+
|
| 86 |
+
def get_face_center(self, face, frame_width, frame_height):
|
| 87 |
+
"""Get the normalized center position of a face.
|
| 88 |
+
|
| 89 |
+
Returns:
|
| 90 |
+
(x_error, y_error) where:
|
| 91 |
+
- x_error: -1.0 (far left) to 1.0 (far right)
|
| 92 |
+
- y_error: -1.0 (top) to 1.0 (bottom)
|
| 93 |
+
"""
|
| 94 |
+
x, y, w, h = face
|
| 95 |
+
cx = x + w / 2
|
| 96 |
+
cy = y + h / 2
|
| 97 |
+
|
| 98 |
+
x_error = (cx - frame_width / 2) / (frame_width / 2)
|
| 99 |
+
y_error = (cy - frame_height / 2) / (frame_height / 2)
|
| 100 |
+
|
| 101 |
+
return x_error, y_error
|
| 102 |
+
|
| 103 |
+
def get_error(self, face, frame_width, frame_height):
|
| 104 |
+
"""Get horizontal error for tracking (backward compatible)."""
|
| 105 |
+
x_err, _ = self.get_face_center(face, frame_width, frame_height)
|
| 106 |
+
return x_err
|
| 107 |
+
|
| 108 |
+
def close(self):
|
| 109 |
+
"""Clean up MediaPipe resources."""
|
| 110 |
+
self.detector.close()
|