eric-rolph commited on
Commit
1f2ecf4
·
1 Parent(s): e90786b

Fix: Manually adding missing python source files

Browse files
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()