""" Service principal Motion pour Skull Pi """ import argparse import signal import sys import time import threading from pathlib import Path from typing import Dict, Any, Optional, Union from .config import MotionConfig from .servo import ServoController from .planner import SCurvePlanner from .pca9685 import PCA9685Driver from .estop import EmergencyStop from .watchdog import OrchestratorWatchdog from .health import HealthMonitor from .mqtt import MotionMQTTClient from .utils import setup_json_logger, get_monotonic_ms, clamp from .exceptions import MotionError, SafetyError class MotionService: """Service principal de pilotage des servomoteurs""" def __init__(self, config: MotionConfig): self.config = config self.logger = setup_json_logger(Path("/opt/Skull/logs/motion.log")) # Flag: l’orchestrateur a-t-il déjà parlé ? self.orchestrator_seen = False # Composants self.pca_driver = PCA9685Driver( address=config.pca9685_addr, frequency=config.pca9685_freq ) self.mqtt_client = MotionMQTTClient(config.mqtt_host, config.mqtt_port) self.health_monitor = HealthMonitor() # Servos et planificateurs self.servos: Dict[str, ServoController] = {} self.planners: Dict[str, SCurvePlanner] = {} # État self._is_running = False self._main_thread: Optional[threading.Thread] = None self._estop_active = False self._components_status: Dict[str, bool] = { "pca9685": False, "mqtt": False, "estop": False, "watchdog": False, } # E-stop et watchdog self.estop = EmergencyStop(config.estop_gpio, self._on_estop_triggered) self.watchdog = OrchestratorWatchdog( config.watchdog_timeout_ms, self._on_watchdog_timeout ) # Positions courantes (pour continuité) self._current_positions: Dict[str, float] = {} # ------------------------------------------------------------------ # # Initialisation # ------------------------------------------------------------------ # def initialize(self) -> bool: """Initialise tous les composants""" self.logger.info("Initialisation du service Motion...") # Initialisation PCA9685 if not self.pca_driver.initialize(): self.logger.error("Échec initialisation PCA9685") return False self._components_status["pca9685"] = True # Initialisation servos (positions au centre) for name, servo_config in self.config.servos.items(): servo = ServoController(name, servo_config, self.config.pca9685_freq) planner = SCurvePlanner(dt_ms=1000 // self.config.loop_freq_hz) self.servos[name] = servo self.planners[name] = planner center_ticks = servo.get_center_ticks() self.pca_driver.set_channel(servo_config.channel, center_ticks) self._current_positions[name] = servo_config.center_deg self.logger.info( f"Servo '{name}' initialisé sur canal {servo_config.channel}" ) # Initialisation MQTT if not self.mqtt_client.initialize(): self.logger.error("Échec initialisation MQTT") return False self._components_status["mqtt"] = True # Callbacks MQTT self.mqtt_client.on_eyes_command = self._handle_eyes_command self.mqtt_client.on_neck_command = self._handle_neck_command self.mqtt_client.on_jaw_command = self._handle_jaw_command self.mqtt_client.on_estop_command = self._handle_estop_command self.mqtt_client.on_orchestrator_state = self._handle_orchestrator_state # Initialisation E-stop if not self.estop.initialize(): self.logger.error("Échec initialisation E-stop") return False self._components_status["estop"] = True # Publication capacités (retained) capabilities = {} for name, servo in self.servos.items(): capabilities[name] = servo.get_capabilities() if not self.mqtt_client.publish_capabilities(capabilities): self.logger.warning("Échec publication capacités") # Démarrage watchdog self.watchdog.start() self._components_status["watchdog"] = True self.logger.info("Service Motion initialisé avec succès") return True # ------------------------------------------------------------------ # # Handlers MQTT / Watchdog / E-stop # ------------------------------------------------------------------ # def _handle_orchestrator_state(self, state: str) -> None: """Traite l'état de l'orchestrateur (nourrit le watchdog et arme le flag).""" # Marquer que l’orchestrateur est présent if not self.orchestrator_seen: self.logger.info("Orchestrateur détecté via MQTT (heartbeat reçu)") self.orchestrator_seen = True # Nourrir le watchdog self.watchdog.feed() self.logger.debug(f"État orchestrateur: {state}") def _handle_eyes_command(self, data: Dict[str, Any]) -> None: """Traite une commande yeux.""" try: h_deg = float(data.get("h", 0.0)) speed = clamp(float(data.get("spd", 1.0)), 0.1, 1.0) # Noms canoniques alignés sur servos.json: eye_left_h / eye_right_h if "eye_left_h" in self.servos: self._move_servo("eye_left_h", h_deg, speed) if "eye_right_h" in self.servos: self._move_servo("eye_right_h", -h_deg, speed) # Symétrique self.logger.debug(f"Commande yeux: h={h_deg}°, spd={speed}") except Exception as e: self.logger.error(f"Erreur commande yeux: {e}") def _handle_neck_command(self, data: Dict[str, Any]) -> None: """Traite une commande cou.""" try: pan_deg = float(data.get("pan", 0.0)) speed = clamp(float(data.get("spd", 1.0)), 0.1, 1.0) lag_ms = int(data.get("lag_ms", 0)) if "neck_pan" in self.servos: self._move_servo("neck_pan", pan_deg, speed, delay_ms=lag_ms) self.logger.debug( f"Commande cou: pan={pan_deg}°, spd={speed}, lag={lag_ms}ms" ) except Exception as e: self.logger.error(f"Erreur commande cou: {e}") def _handle_jaw_command(self, data: Dict[str, Any]) -> None: """Traite une commande mâchoire.""" try: deg = float(data.get("deg", 0.0)) speed = clamp(float(data.get("spd", 1.0)), 0.1, 1.0) if "jaw" in self.servos: self._move_servo("jaw", deg, speed) self.logger.debug(f"Commande mâchoire: deg={deg}°, spd={speed}") except Exception as e: self.logger.error(f"Erreur commande mâchoire: {e}") def _handle_estop_command(self, command: Union[str, Dict[str, Any]]) -> None: """Traite une commande E-stop MQTT (string ou dict).""" try: if isinstance(command, dict): cmd = str(command.get("cmd", "")).lower() else: cmd = str(command).lower() if cmd == "on": self.logger.warning("E-stop activé via MQTT") self._trigger_estop() except Exception as e: self.logger.error(f"Erreur commande E-stop MQTT: {e}") def _on_estop_triggered(self) -> None: """Callback déclenchement E-stop (physique).""" self._trigger_estop() def _on_watchdog_timeout(self) -> None: """Callback timeout watchdog.""" self.logger.warning("Watchdog timeout - retour position centre") self._return_to_center() # ------------------------------------------------------------------ # # Mouvements # ------------------------------------------------------------------ # def _move_servo( self, servo_name: str, target_deg: float, speed: float, delay_ms: int = 0 ) -> None: """Démarre un mouvement servo avec planification S-curve.""" if self._estop_active: self.logger.warning(f"Mouvement {servo_name} ignoré (E-stop actif)") return servo = self.servos.get(servo_name) planner = self.planners.get(servo_name) if not servo or not planner: self.logger.error(f"Servo/planificateur '{servo_name}' introuvable") return # Clamp et validation target_clamped, was_clamped = servo.validate_and_clamp_deg(target_deg) if was_clamped: self.logger.warning( f"Commande {servo_name} clampée: {target_deg}° → {target_clamped}°" ) # Position de départ start_pos = self._current_positions.get(servo_name, servo.config.center_deg) # Planification mouvement planner.start_movement(start_pos, target_clamped, speed, delay_ms) self.logger.info( f"Mouvement {servo_name}: {start_pos:.1f}° → {target_clamped:.1f}° (spd={speed:.2f})" ) def _return_to_center(self) -> None: """Retourne tous les servos au centre.""" self.logger.info("Retour position centre") for servo_name, servo in self.servos.items(): try: center_ticks = servo.get_center_ticks() self.pca_driver.set_channel(servo.config.channel, center_ticks) self._current_positions[servo_name] = servo.config.center_deg # Arrêter les mouvements en cours planner = self.planners.get(servo_name) if planner: planner.start_movement( servo.config.center_deg, servo.config.center_deg, 0.3 ) except Exception as e: self.logger.error(f"Erreur recentrage {servo_name}: {e}") # Reset E-stop après recentrage self._estop_active = False self.mqtt_client.publish_estop_status(False) def _trigger_estop(self) -> None: """Déclenche l'arrêt d'urgence.""" self._estop_active = True # Arrêt immédiat PWM self.pca_driver.stop_all_channels() # Publication statut self.mqtt_client.publish_estop_status(True) # Retour centre sécurisé après un délai court threading.Timer(0.1, self._return_to_center).start() self.logger.warning("E-STOP DÉCLENCHÉ - Arrêt et recentrage") # ------------------------------------------------------------------ # # Boucle principale # ------------------------------------------------------------------ # def _main_loop(self) -> None: """Boucle principale du service""" self.logger.info("Boucle principale démarrée") loop_interval_ms = 1000 // self.config.loop_freq_hz while self._is_running: loop_start_ms = get_monotonic_ms() try: # ✅ Tant que l’orchestrateur n’a pas parlé, nourrir le watchdog # pour éviter les faux timeouts au démarrage. if not self.orchestrator_seen: self.watchdog.feed() # Mise à jour positions selon planifications self._update_servo_positions() # Monitoring santé self.health_monitor.tick_loop() if self.health_monitor.should_publish_health(): health_data = self.health_monitor.get_health_data( self._components_status ) self.mqtt_client.publish_health(health_data) # Contrôle timing elapsed_ms = get_monotonic_ms() - loop_start_ms sleep_ms = max(0, loop_interval_ms - elapsed_ms) if sleep_ms > 0: time.sleep(sleep_ms / 1000.0) elif elapsed_ms > loop_interval_ms * 1.5: self.logger.warning( f"Boucle lente: {elapsed_ms}ms (cible: {loop_interval_ms}ms)" ) except Exception as e: self.logger.error(f"Erreur boucle principale: {e}") time.sleep(0.1) # Éviter une boucle folle self.logger.info("Boucle principale arrêtée") def _update_servo_positions(self) -> None: """Met à jour les positions des servos selon les planifications""" if self._estop_active: return for servo_name, planner in self.planners.items(): servo = self.servos.get(servo_name) if not servo: continue # Position courante selon planification current_pos = planner.get_current_position() if current_pos is None: continue # Conversion et écriture PCA try: target_ticks = servo.deg_to_ticks(current_pos) # Éviter les écritures inutiles last_ticks = self.pca_driver.get_channel_value(servo.config.channel) if last_ticks != target_ticks: success = self.pca_driver.set_channel( servo.config.channel, target_ticks ) if success: self._current_positions[servo_name] = current_pos else: self.logger.error( f"Échec écriture {servo_name} canal {servo.config.channel}" ) except Exception as e: self.logger.error(f"Erreur mise à jour {servo_name}: {e}") # ------------------------------------------------------------------ # # Cycle de vie # ------------------------------------------------------------------ # def start(self) -> bool: """Démarre le service""" if self._is_running: self.logger.warning("Service déjà en cours") return True if not self.initialize(): self.logger.error("Échec initialisation service") return False self._is_running = True # Thread principal self._main_thread = threading.Thread(target=self._main_loop, daemon=True) self._main_thread.start() self.logger.info("Service Motion démarré") return True def stop(self) -> None: """Arrête le service""" if not self._is_running: return self.logger.info("Arrêt du service Motion...") self._is_running = False # Arrêt composants self.watchdog.stop() # Retour centre avant arrêt self._return_to_center() time.sleep(0.5) # Laisser le temps # Arrêt PWM self.pca_driver.stop_all_channels() # Attendre thread principal if self._main_thread: self._main_thread.join(timeout=2.0) # Cleanup self.mqtt_client.cleanup() self.pca_driver.cleanup() self.estop.cleanup() self.logger.info("Service Motion arrêté") def get_status(self) -> Dict[str, Any]: """Retourne le statut du service""" return { "running": self._is_running, "estop_active": self._estop_active, "components": self._components_status.copy(), "current_positions": self._current_positions.copy(), "mqtt_connected": self.mqtt_client.is_connected(), "orchestrator_seen": self.orchestrator_seen, } def create_signal_handler(service: MotionService): """Crée un gestionnaire de signaux pour arrêt propre""" def signal_handler(signum, frame): print(f"\nSignal {signum} reçu, arrêt du service...") service.stop() sys.exit(0) return signal_handler def main(): """Point d'entrée principal""" parser = argparse.ArgumentParser(description="Skull Pi Motion Service") parser.add_argument( "--config", type=Path, default="/opt/Skull/config/servos.json", help="Fichier de configuration servos", ) parser.add_argument( "--env", type=Path, default="/opt/Skull/config/.env", help="Fichier variables d'environnement", ) parser.add_argument( "--log-level", choices=["DEBUG", "INFO", "WARNING", "ERROR"], default="INFO", help="Niveau de log", ) args = parser.parse_args() # Configuration logging (optionnel si ton setup_json_logger gère déjà) import logging # noqa: WPS433 log_level = getattr(logging, args.log_level, logging.INFO) logging.getLogger().setLevel(log_level) try: # Chargement configuration config = MotionConfig.from_files(args.config, args.env) # Création service service = MotionService(config) # Gestionnaire signaux signal_handler = create_signal_handler(service) signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) # Démarrage if not service.start(): print("Échec démarrage service", file=sys.stderr) sys.exit(1) print("Service Motion démarré. Ctrl+C pour arrêter.") # Attente infinie try: while True: time.sleep(1.0) # Vérification santé status = service.get_status() if not status["running"]: print("Service arrêté de manière inattendue", file=sys.stderr) break except KeyboardInterrupt: pass except Exception as e: print(f"Erreur fatale: {e}", file=sys.stderr) sys.exit(1) finally: if "service" in locals(): service.stop() if __name__ == "__main__": main()