"""Mapping et émission commandes MQTT pour eyes/neck.""" import json import logging from typing import Optional, Dict, Any from .smoothing import SmoothedPose from .mqtt import MQTTClient logger = logging.getLogger(__name__) class MotionCommandMapper: """Mapper pour convertir poses en commandes motion eyes/neck.""" def __init__( self, mqtt_client: MQTTClient, gain_eyes: float = 0.9, gain_neck: float = 0.6, h_max_deg: float = 45.0, pan_max_deg: float = 90.0, ): self.mqtt_client = mqtt_client self.gain_eyes = gain_eyes self.gain_neck = gain_neck self.h_max_deg = h_max_deg self.pan_max_deg = pan_max_deg self.enabled = True # Cache dernières commandes pour éviter spam self._last_eyes_cmd: Optional[Dict[str, Any]] = None self._last_neck_cmd: Optional[Dict[str, Any]] = None def send_eyes_command(self, pose: SmoothedPose) -> None: """Envoie commande eyes immédiate.""" if not self.enabled: return # Calcul h basé sur position horizontale du visage # pose.yaw est en radians, on utilise une approche simplifiée # h = gain_eyes * ((cx-0.5)*2) * H_MAX_DEG # Ici on utilise yaw comme proxy pour position horizontale h_normalized = pose.yaw / (0.8) # Normaliser yaw [-0.8, 0.8] rad -> [-1, 1] h_normalized = max(-1.0, min(1.0, h_normalized)) h_deg = self.gain_eyes * h_normalized * self.h_max_deg # Vitesse basée sur confiance speed = max(0.3, min(1.0, pose.confidence)) eyes_cmd = {"h": round(h_deg, 2), "spd": round(speed, 2)} # Éviter spam si commande identique if eyes_cmd != self._last_eyes_cmd: try: self.mqtt_client.publish("eyes/cmd", json.dumps(eyes_cmd)) self._last_eyes_cmd = eyes_cmd logger.debug(f"Eyes cmd: {eyes_cmd}") except Exception as e: logger.error(f"Erreur envoi eyes/cmd: {e}") def send_neck_command(self, pose: SmoothedPose, lag_ms: int) -> None: """Envoie commande neck avec retard.""" if not self.enabled: return # Calcul pan basé sur yaw pan_normalized = pose.yaw / (0.8) # Normaliser yaw pan_normalized = max(-1.0, min(1.0, pan_normalized)) pan_deg = self.gain_neck * pan_normalized * self.pan_max_deg # Vitesse basée sur confiance (plus lente que eyes) speed = max(0.2, min(0.8, pose.confidence * 0.8)) neck_cmd = {"pan": round(pan_deg, 2), "spd": round(speed, 2), "lag_ms": lag_ms} # Éviter spam if neck_cmd != self._last_neck_cmd: try: self.mqtt_client.publish("neck/cmd", json.dumps(neck_cmd)) self._last_neck_cmd = neck_cmd logger.debug(f"Neck cmd: {neck_cmd} (lag: {lag_ms}ms)") except Exception as e: logger.error(f"Erreur envoi neck/cmd: {e}") def update_gains(self, gain_eyes: float, gain_neck: float) -> None: """Met à jour les gains de mapping.""" self.gain_eyes = max(0.0, min(2.0, gain_eyes)) self.gain_neck = max(0.0, min(2.0, gain_neck)) logger.debug( f"Gains mis à jour - eyes: {self.gain_eyes}, neck: {self.gain_neck}" ) def set_enabled(self, enabled: bool) -> None: """Active/désactive l'envoi de commandes.""" if self.enabled != enabled: self.enabled = enabled logger.info(f"Motion commands {'enabled' if enabled else 'disabled'}") if not enabled: # Reset cache self._last_eyes_cmd = None self._last_neck_cmd = None class BBoxBasedMapper: """Mapper alternatif basé directement sur bbox (cx, cy) plutôt que yaw/pitch.""" def __init__( self, mqtt_client: MQTTClient, gain_eyes: float = 0.9, gain_neck: float = 0.6 ): self.mqtt_client = mqtt_client self.gain_eyes = gain_eyes self.gain_neck = gain_neck self.enabled = True self._last_eyes_cmd: Optional[Dict[str, Any]] = None self._last_neck_cmd: Optional[Dict[str, Any]] = None def send_eyes_command_from_bbox( self, cx: float, cy: float, confidence: float ) -> None: """Envoie commande eyes basée sur centre bbox.""" if not self.enabled: return # h = gain_eyes * ((cx-0.5)*2) * H_MAX_DEG h_normalized = (cx - 0.5) * 2.0 # [-1, 1] h_deg = self.gain_eyes * h_normalized * 45.0 # Max ±45° speed = max(0.3, min(1.0, confidence)) eyes_cmd = {"h": round(h_deg, 2), "spd": round(speed, 2)} if eyes_cmd != self._last_eyes_cmd: try: self.mqtt_client.publish("eyes/cmd", json.dumps(eyes_cmd)) self._last_eyes_cmd = eyes_cmd logger.debug(f"Eyes cmd (bbox): {eyes_cmd}") except Exception as e: logger.error(f"Erreur envoi eyes/cmd: {e}") def send_neck_command_from_bbox( self, cx: float, cy: float, confidence: float, lag_ms: int ) -> None: """Envoie commande neck basée sur centre bbox.""" if not self.enabled: return # pan = gain_neck * ((cx-0.5)*2) * PAN_MAX_DEG pan_normalized = (cx - 0.5) * 2.0 # [-1, 1] pan_deg = self.gain_neck * pan_normalized * 90.0 # Max ±90° speed = max(0.2, min(0.8, confidence * 0.8)) neck_cmd = {"pan": round(pan_deg, 2), "spd": round(speed, 2), "lag_ms": lag_ms} if neck_cmd != self._last_neck_cmd: try: self.mqtt_client.publish("neck/cmd", json.dumps(neck_cmd)) self._last_neck_cmd = neck_cmd logger.debug(f"Neck cmd (bbox): {neck_cmd}") except Exception as e: logger.error(f"Erreur envoi neck/cmd: {e}") def update_gains(self, gain_eyes: float, gain_neck: float) -> None: """Met à jour les gains.""" self.gain_eyes = max(0.0, min(2.0, gain_eyes)) self.gain_neck = max(0.0, min(2.0, gain_neck)) def set_enabled(self, enabled: bool) -> None: """Active/désactive l'envoi.""" if self.enabled != enabled: self.enabled = enabled logger.info(f"BBox motion commands {'enabled' if enabled else 'disabled'}") class CompositeMapper: """Mapper composite utilisant les deux approches selon disponibilité.""" def __init__(self, mqtt_client: MQTTClient): self.pose_mapper = MotionCommandMapper(mqtt_client) self.bbox_mapper = BBoxBasedMapper(mqtt_client) self.prefer_pose = True # Préférer pose si disponible def send_commands( self, pose: Optional[SmoothedPose], bbox: Optional[tuple], confidence: float, neck_lag_ms: int, ) -> None: """Envoie commandes en utilisant la meilleure approche disponible.""" # Essayer avec pose d'abord si préféré et disponible if self.prefer_pose and pose is not None: self.pose_mapper.send_eyes_command(pose) self.pose_mapper.send_neck_command(pose, neck_lag_ms) return # Fallback sur bbox si disponible if bbox is not None: cx, cy, w, h = bbox self.bbox_mapper.send_eyes_command_from_bbox(cx, cy, confidence) self.bbox_mapper.send_neck_command_from_bbox( cx, cy, confidence, neck_lag_ms ) return # Si pose disponible en dernier recours if pose is not None: self.pose_mapper.send_eyes_command(pose) self.pose_mapper.send_neck_command(pose, neck_lag_ms) def update_config(self, gain_eyes: float, gain_neck: float, enabled: bool) -> None: """Met à jour configuration des deux mappers.""" self.pose_mapper.update_gains(gain_eyes, gain_neck) self.pose_mapper.set_enabled(enabled) self.bbox_mapper.update_gains(gain_eyes, gain_neck) self.bbox_mapper.set_enabled(enabled) def set_pose_preference(self, prefer_pose: bool) -> None: """Définit préférence entre pose et bbox.""" self.prefer_pose = prefer_pose