"""Service principal Vision - Boucle capture, processing et MQTT.""" import sys import time import json import logging import signal import argparse from typing import Optional, Dict, Any from threading import Thread, Event from .config import ConfigManager from .camera import create_camera from .mp import create_processor from .tracker import FaceTracker, TargetSelector from .motion_score import MotionScoreCalculator from .smoothing import SmoothingManager from .mapper import CompositeMapper from .mqtt import MQTTClient, VisionMQTTPublisher, ConfigSubscriber from .health import HealthManager # Configuration logging logging.basicConfig( level=logging.INFO, format='{"timestamp": "%(asctime)s", "level": "%(levelname)s", "module": "%(name)s", "message": "%(message)s"}', handlers=[ logging.StreamHandler(), logging.FileHandler("/opt/Skull/logs/vision.log"), ], ) logger = logging.getLogger(__name__) class VisionService: """Service principal de vision temps réel.""" def __init__(self, mock_mode: bool = False): self.mock_mode = mock_mode self.running = False self.shutdown_event = Event() # Configuration self.config_manager = ConfigManager() # Composants principaux self.camera = None self.mp_processor = None self.face_tracker = None self.target_selector = None self.motion_calculator = None self.smoothing_manager = None self.mapper = None # MQTT self.mqtt_client = None self.mqtt_publisher = None self.config_subscriber = None # Santé et monitoring self.health_manager = HealthManager() # Thread de capture self.capture_thread: Optional[Thread] = None self.publish_thread: Optional[Thread] = None # État interne self.current_target = None self.last_capabilities_publish = 0.0 logger.info(f"VisionService initialisé (mock_mode: {mock_mode})") def initialize(self) -> bool: """Initialise tous les composants.""" try: # MQTT if not self._init_mqtt(): return False # Caméra if not self._init_camera(): return False # MediaPipe if not self._init_mediapipe(): return False # Tracking & sélection self._init_tracking() # Motion score self._init_motion_score() # Lissage self._init_smoothing() # Mapping commandes self._init_mapping() # Publier capabilities initial self._publish_capabilities() # Callbacks watchdog self.health_manager.add_restart_callback(self._restart_components) logger.info("VisionService initialisé avec succès") return True except Exception as e: logger.error(f"Erreur initialisation VisionService: {e}") return False def _init_mqtt(self) -> bool: """Initialise client MQTT.""" try: self.mqtt_client = MQTTClient() if not self.mqtt_client.connect(): logger.error("Échec connexion MQTT") return False self.mqtt_publisher = VisionMQTTPublisher(self.mqtt_client) self.config_subscriber = ConfigSubscriber(self.mqtt_client) # Abonnements config self.config_subscriber.setup_subscriptions( self._on_vision_config_update, self._on_gaze_config_update ) self.health_manager.update_component_status(mqtt=True) return True except Exception as e: logger.error(f"Erreur init MQTT: {e}") self.health_manager.update_component_status(mqtt=False) return False def _init_camera(self) -> bool: """Initialise caméra.""" try: self.camera = create_camera( self.config_manager.env.width, self.config_manager.env.height, self.config_manager.env.fps, mock=self.mock_mode, ) if not self.camera.open(): logger.error("Échec ouverture caméra") self.health_manager.update_component_status(camera=False) return False self.health_manager.update_component_status(camera=True) return True except Exception as e: logger.error(f"Erreur init caméra: {e}") self.health_manager.update_component_status(camera=False) return False def _init_mediapipe(self) -> bool: """Initialise MediaPipe.""" try: self.mp_processor = create_processor( self.config_manager.env.min_detection_confidence, self.config_manager.env.min_tracking_confidence, mock=self.mock_mode, ) self.health_manager.update_component_status(mediapipe=True) return True except Exception as e: logger.error(f"Erreur init MediaPipe: {e}") self.health_manager.update_component_status(mediapipe=False) # Continuer sans MediaPipe (mode dégradé) return True def _init_tracking(self) -> None: """Initialise tracking et sélection.""" self.face_tracker = FaceTracker( max_targets=self.config_manager.gaze.max_targets, max_age_seconds=2.0 ) self.target_selector = TargetSelector( policy=self.config_manager.gaze.policy, dwell_ms=self.config_manager.gaze.dwell_ms, ) def _init_motion_score(self) -> None: """Initialise calculateur mouvement.""" self.motion_calculator = MotionScoreCalculator( threshold=self.config_manager.vision.motion_threshold ) def _init_smoothing(self) -> None: """Initialise lissage.""" self.smoothing_manager = SmoothingManager( eyes_alpha=self.config_manager.vision.eyes_alpha, neck_alpha=self.config_manager.vision.neck_alpha, neck_lag_ms=self.config_manager.vision.neck_lag_ms, ) def _init_mapping(self) -> None: """Initialise mapping commandes.""" if self.mqtt_client: self.mapper = CompositeMapper(self.mqtt_client) self.mapper.update_config( gain_eyes=self.config_manager.gaze.gain_eyes, gain_neck=self.config_manager.gaze.gain_neck, enabled=self.config_manager.vision.publish_motion_cmds, ) def start(self) -> None: """Démarre le service.""" if self.running: logger.warning("Service déjà démarré") return self.running = True self.shutdown_event.clear() # Démarrer threads self.capture_thread = Thread(target=self._capture_loop, daemon=True) self.capture_thread.start() self.publish_thread = Thread(target=self._publish_loop, daemon=True) self.publish_thread.start() logger.info("VisionService démarré") def stop(self) -> None: """Arrête le service.""" if not self.running: return logger.info("Arrêt VisionService...") self.running = False self.shutdown_event.set() # Attendre arrêt threads if self.capture_thread: self.capture_thread.join(timeout=2.0) if self.publish_thread: self.publish_thread.join(timeout=2.0) # Fermer ressources if self.camera: self.camera.close() if self.mqtt_client: self.mqtt_client.disconnect() logger.info("VisionService arrêté") def _capture_loop(self) -> None: """Boucle capture et traitement.""" logger.info("Boucle capture démarrée") while self.running: try: # Check watchdog if self.health_manager.check_watchdog(): logger.info("Redémarrage par watchdog") continue # Timestamp début pipeline pipeline_start = self.health_manager.record_pipeline_start() # Capture frame frame = self.camera.read_frame() if self.camera else None if frame is None: self.health_manager.record_error() time.sleep(0.1) # Éviter boucle infinie continue self.health_manager.record_frame_capture() # Processing MediaPipe detections = [] if self.mp_processor: detections = self.mp_processor.process_frame(frame) # Tracking tracked_faces = [] if self.face_tracker and detections: tracked_faces = self.face_tracker.update(detections) # Sélection cible current_target = None if self.target_selector and tracked_faces: current_target = self.target_selector.select_target(tracked_faces) self.current_target = current_target # Score mouvement motion_state = None if self.motion_calculator: motion_state = self.motion_calculator.calculate_motion_score( tracked_faces ) # Lissage et mapping si cible if current_target and self.smoothing_manager and self.mapper: eyes_pose, neck_pose = self.smoothing_manager.process_pose( current_target.detection.yaw or 0.0, current_target.detection.pitch or 0.0, current_target.detection.confidence, current_target.id, ) # Commandes motion si activées if self.config_manager.vision.publish_motion_cmds: if eyes_pose: self.mapper.pose_mapper.send_eyes_command(eyes_pose) if neck_pose: self.mapper.pose_mapper.send_neck_command( neck_pose, self.config_manager.vision.neck_lag_ms ) # Mesurer latence pipeline self.health_manager.record_pipeline_end(pipeline_start) except Exception as e: logger.error(f"Erreur boucle capture: {e}") self.health_manager.record_error() time.sleep(0.1) logger.info("Boucle capture terminée") def _publish_loop(self) -> None: """Boucle publication MQTT.""" logger.info("Boucle publication démarrée") last_publish = 0.0 publish_interval = 1.0 / 15.0 # 15 Hz max while self.running: try: current_time = time.time() if current_time - last_publish < publish_interval: time.sleep(0.01) continue # Publier targets self._publish_targets() # Publier pose cible self._publish_pose() # Publier motion self._publish_motion() # Publier capabilities périodiquement if current_time - self.last_capabilities_publish > 30.0: self._publish_capabilities() # Publier health périodiquement if int(current_time) % 2 == 0: # Toutes les 2s self._publish_health() last_publish = current_time except Exception as e: logger.error(f"Erreur boucle publication: {e}") time.sleep(0.1) logger.info("Boucle publication terminée") def _publish_targets(self) -> None: """Publie vision/targets.""" if not self.mqtt_publisher or not self.face_tracker: return tracked_faces = self.face_tracker.get_tracked_faces() targets_data = {"ts_ms": int(time.time() * 1000), "targets": []} for face in tracked_faces: target = { "id": face.id, "conf": round(face.detection.confidence, 2), "bbox": { "cx": round(face.detection.bbox[0], 3), "cy": round(face.detection.bbox[1], 3), "w": round(face.detection.bbox[2], 3), "h": round(face.detection.bbox[3], 3), }, } # Ajouter angles si disponibles if face.detection.yaw is not None: target["yaw"] = round(face.detection.yaw, 3) if face.detection.pitch is not None: target["pitch"] = round(face.detection.pitch, 3) if face.detection.roll is not None: target["roll"] = round(face.detection.roll, 3) targets_data["targets"].append(target) self.mqtt_publisher.publish_targets(targets_data) def _publish_pose(self) -> None: """Publie vision/pose.""" if not self.mqtt_publisher or not self.current_target: return pose_data = { "ts_ms": int(time.time() * 1000), "yaw": round(self.current_target.detection.yaw or 0.0, 3), "pitch": round(self.current_target.detection.pitch or 0.0, 3), "conf": round(self.current_target.detection.confidence, 2), "target_id": self.current_target.id, } self.mqtt_publisher.publish_pose(pose_data) def _publish_motion(self) -> None: """Publie vision/motion.""" if not self.mqtt_publisher or not self.motion_calculator: return motion_state = self.motion_calculator.current_motion_state motion_data = { "ts_ms": int(motion_state.timestamp * 1000), "active": motion_state.active, "score": round(motion_state.score, 2), "reason": motion_state.reason, } self.mqtt_publisher.publish_motion(motion_data) def _publish_capabilities(self) -> None: """Publie vision/capabilities (retained).""" if not self.mqtt_publisher: return capabilities = { "mediapipe": self.mp_processor is not None and hasattr(self.mp_processor, "mesh_available"), "fps": self.config_manager.env.fps, "res": self.config_manager.env.resolution, "multi_face": True, } self.mqtt_publisher.publish_capabilities(capabilities) self.last_capabilities_publish = time.time() def _publish_health(self) -> None: """Publie vision/health.""" if not self.mqtt_publisher: return health_summary = self.health_manager.get_health_summary() self.mqtt_publisher.publish_health(health_summary) def _on_vision_config_update(self, config_data: Dict[str, Any]) -> None: """Callback mise à jour config vision.""" logger.info(f"Mise à jour config vision: {config_data}") # Mettre à jour config self.config_manager.update_vision_config(config_data) # Appliquer changements if self.mp_processor: self.mp_processor.update_confidence_thresholds( self.config_manager.vision.min_detection_confidence, self.config_manager.vision.min_tracking_confidence, ) if self.motion_calculator: self.motion_calculator.update_threshold( self.config_manager.vision.motion_threshold ) if self.smoothing_manager: self.smoothing_manager.update_config( self.config_manager.vision.eyes_alpha, self.config_manager.vision.neck_alpha, self.config_manager.vision.neck_lag_ms, ) if self.mapper: self.mapper.update_config( self.config_manager.gaze.gain_eyes, self.config_manager.gaze.gain_neck, self.config_manager.vision.publish_motion_cmds, ) def _on_gaze_config_update(self, config_data: Dict[str, Any]) -> None: """Callback mise à jour config gaze.""" logger.info(f"Mise à jour config gaze: {config_data}") # Mettre à jour config self.config_manager.update_gaze_config(config_data) # Appliquer changements if self.face_tracker: self.face_tracker.max_targets = self.config_manager.gaze.max_targets if self.target_selector: self.target_selector.update_config( self.config_manager.gaze.policy, self.config_manager.gaze.dwell_ms ) if self.mapper: self.mapper.update_config( self.config_manager.gaze.gain_eyes, self.config_manager.gaze.gain_neck, self.config_manager.vision.publish_motion_cmds, ) def _restart_components(self) -> None: """Callback redémarrage composants (watchdog).""" logger.warning("Redémarrage composants...") try: # Réinitialiser caméra if self.camera: self.camera.close() time.sleep(0.5) self._init_camera() # Réinitialiser MediaPipe self._init_mediapipe() # Reset lissage if self.smoothing_manager: self.smoothing_manager.reset() # Republier capabilities self._publish_capabilities() logger.info("Redémarrage composants terminé") except Exception as e: logger.error(f"Erreur redémarrage composants: {e}") def create_vision_service(mock_mode: bool = False) -> VisionService: """Factory pour créer service vision.""" return VisionService(mock_mode=mock_mode) def run_benchmark(duration: int = 30) -> None: """Lance benchmark performance.""" logger.info(f"Démarrage benchmark ({duration}s)") service = create_vision_service(mock_mode=True) if not service.initialize(): logger.error("Échec initialisation service pour benchmark") return service.start() # Collecter métriques start_time = time.time() try: while time.time() - start_time < duration: time.sleep(1.0) # Afficher métriques chaque 5s if int(time.time() - start_time) % 5 == 0: health_status = service.health_manager.get_health_status() print( f"FPS: {health_status.fps_pipeline:.1f}, " f"Latence P50: {health_status.latency_p50:.1f}ms, " f"OK: {health_status.ok}" ) finally: service.stop() # Rapport final final_report = service.health_manager.get_full_report() print("\n=== RAPPORT BENCHMARK ===") print(json.dumps(final_report, indent=2)) def setup_signal_handlers(service: VisionService) -> None: """Configure gestionnaires de signaux.""" def signal_handler(signum, frame): logger.info(f"Signal {signum} reçu, arrêt service...") service.stop() sys.exit(0) signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) def main(): """Point d'entrée principal.""" parser = argparse.ArgumentParser(description="Service Vision Skull Pi") parser.add_argument( "--mock", action="store_true", help="Mode simulation (sans caméra/MediaPipe)" ) parser.add_argument("--bench", action="store_true", help="Mode benchmark") parser.add_argument( "--duration", type=int, default=30, help="Durée benchmark (secondes)" ) parser.add_argument( "--log-level", choices=["DEBUG", "INFO", "WARNING", "ERROR"], default="INFO", help="Niveau de log", ) args = parser.parse_args() # Configuration logging logging.getLogger().setLevel(getattr(logging, args.log_level)) if args.bench: run_benchmark(args.duration) return # Service normal logger.info("Démarrage service Vision Skull Pi") service = create_vision_service(mock_mode=args.mock) if not service.initialize(): logger.error("Échec initialisation service") sys.exit(1) # Gestionnaires signaux setup_signal_handlers(service) # Démarrer service service.start() try: # Boucle principale while service.running: time.sleep(1.0) # Log métriques périodiquement if int(time.time()) % 30 == 0: # Toutes les 30s health = service.health_manager.get_health_summary() logger.info(f"Santé service: {health}") except KeyboardInterrupt: logger.info("Interruption clavier") finally: service.stop() logger.info("Service Vision Skull Pi terminé") if __name__ == "__main__": main()