Funciona todo sin modulo_imu

modulo_slam.py

@@ -0,0 +1,196 @@
+import math
+import numpy as np
+
+
+class EvasorObstaculos:
+    """
+    Módulo de evasión de obstáculos para robot con cámara RealSense D415.
+    Analiza el escaneo de profundidad y genera comandos de movimiento seguros.
+    """
+
+    # ── Zonas del campo visual (en fracción de los 640 píxeles) ──────────────
+    ZONA_IZQ   = (0,   213)   # píxeles 0–212
+    ZONA_CENT  = (213, 427)   # píxeles 213–426
+    ZONA_DER   = (427, 640)   # píxeles 427–639
+
+    # ── Umbrales de distancia (metros) ──────────────────────────────────────
+    DIST_PELIGRO  = 0.45   # detener / maniobrar urgente
+    DIST_PRECAUCION = 0.80 # reducir velocidad / preparar giro
+    DIST_LIBRE    = 1.20   # camino despejado
+
+    # ── Comandos de salida ───────────────────────────────────────────────────
+    CMD_ADELANTE      = "ADELANTE"
+    CMD_ADELANTE_LENTO= "ADELANTE_LENTO"
+    CMD_GIRO_IZQ      = "GIRAR_IZQUIERDA"
+    CMD_GIRO_DER      = "GIRAR_DERECHA"
+    CMD_RETROCEDER    = "RETROCEDER"
+    CMD_DETENIDO      = "DETENIDO"
+
+    def __init__(self,
+                 dist_peligro: float   = None,
+                 dist_precaucion: float = None,
+                 dist_libre: float     = None):
+        self.dist_peligro    = dist_peligro    or self.DIST_PELIGRO
+        self.dist_precaucion = dist_precaucion or self.DIST_PRECAUCION
+        self.dist_libre      = dist_libre      or self.DIST_LIBRE
+
+        # Último estado para logging / depuración
+        self.ultimo_estado: dict = {}
+
+    # ────────────────────────────────────────────────────────────────────────
+    # Método principal
+    # ────────────────────────────────────────────────────────────────────────
+    def decidir_movimiento(self, escaner: np.ndarray) -> dict:
+        """
+        Recibe el array de 640 distancias (metros) de la cámara y
+        devuelve un diccionario con el comando y métricas de la decisión.
+
+        Returns
+        -------
+        {
+            "comando"    : str,   # CMD_* constante
+            "dist_izq"   : float, # distancia mínima zona izquierda
+            "dist_centro": float, # distancia mínima zona central
+            "dist_der"   : float, # distancia mínima zona derecha
+            "obstaculo"  : bool,  # True si hay peligro inminente
+            "razon"      : str,   # explicación legible
+        }
+        """
+        if escaner is None or len(escaner) != 640:
+            return self._estado(self.CMD_DETENIDO, 0, 0, 0, True,
+                                "Escáner no disponible")
+
+        # ── 1. Distancias mínimas por zona (ignorar lecturas inválidas) ─────
+        dist_izq    = self._min_valida(escaner, *self.ZONA_IZQ)
+        dist_centro = self._min_valida(escaner, *self.ZONA_CENT)
+        dist_der    = self._min_valida(escaner, *self.ZONA_DER)
+
+        # ── 2. Árbol de decisión ─────────────────────────────────────────────
+        cmd, razon = self._arbol_decision(dist_izq, dist_centro, dist_der)
+
+        hay_peligro = (dist_centro < self.dist_peligro or
+                       dist_izq    < self.dist_peligro or
+                       dist_der    < self.dist_peligro)
+
+        self.ultimo_estado = self._estado(
+            cmd, dist_izq, dist_centro, dist_der, hay_peligro, razon)
+        return self.ultimo_estado
+
+    # ────────────────────────────────────────────────────────────────────────
+    # Árbol de decisión
+    # ────────────────────────────────────────────────────────────────────────
+    def _arbol_decision(self,
+                        izq: float,
+                        centro: float,
+                        der: float) -> tuple[str, str]:
+        """Lógica de prioridad para seleccionar comando."""
+
+        # ── Zona central bloqueada ───────────────────────────────────────────
+        if centro < self.dist_peligro:
+
+            # Ambos lados bloqueados → retroceder
+            if izq < self.dist_peligro and der < self.dist_peligro:
+                return self.CMD_RETROCEDER, \
+                       f"Todos los lados bloqueados ({centro:.2f}m)"
+
+            # Izquierda libre → girar izquierda
+            if izq >= der:
+                return self.CMD_GIRO_IZQ, \
+                       f"Centro bloqueado ({centro:.2f}m), izq más libre ({izq:.2f}m)"
+
+            # Derecha libre → girar derecha
+            return self.CMD_GIRO_DER, \
+                   f"Centro bloqueado ({centro:.2f}m), der más libre ({der:.2f}m)"
+
+        # ── Precaución central ───────────────────────────────────────────────
+        if centro < self.dist_precaucion:
+
+            if izq > self.dist_libre and der <= self.dist_precaucion:
+                return self.CMD_GIRO_IZQ, \
+                       f"Precaución centro ({centro:.2f}m), girando izq"
+
+            if der > self.dist_libre and izq <= self.dist_precaucion:
+                return self.CMD_GIRO_DER, \
+                       f"Precaución centro ({centro:.2f}m), girando der"
+
+            return self.CMD_ADELANTE_LENTO, \
+                   f"Precaución ({centro:.2f}m), avanzando despacio"
+
+        # ── Centro libre — revisar laterales ────────────────────────────────
+        if izq < self.dist_peligro:
+            return self.CMD_GIRO_DER, \
+                   f"Obstáculo lateral izq ({izq:.2f}m)"
+
+        if der < self.dist_peligro:
+            return self.CMD_GIRO_IZQ, \
+                   f"Obstáculo lateral der ({der:.2f}m)"
+
+        # ── Camino despejado ─────────────────────────────────────────────────
+        return self.CMD_ADELANTE, \
+               f"Camino libre (centro={centro:.2f}m)"
+
+    # ────────────────────────────────────────────────────────────────────────
+    # Utilidades
+    # ────────────────────────────────────────────────────────────────────────
+    def _min_valida(self, escaner: np.ndarray,
+                    inicio: int, fin: int) -> float:
+        """Distancia mínima en la zona [inicio, fin) ignorando valores nulos."""
+        zona = escaner[inicio:fin]
+        validos = zona[(zona > 0.05) & (zona < 10.0)]
+        return float(np.min(validos)) if len(validos) else 10.0
+
+    @staticmethod
+    def _estado(cmd, izq, centro, der, peligro, razon) -> dict:
+        return {
+            "comando"    : cmd,
+            "dist_izq"   : round(float(izq),    3),
+            "dist_centro": round(float(centro),  3),
+            "dist_der"   : round(float(der),     3),
+            "obstaculo"  : peligro,
+            "razon"      : razon,
+        }
+
+    # ────────────────────────────────────────────────────────────────────────
+    # Visualización en el mapa SLAM
+    # ────────────────────────────────────────────────────────────────────────
+    def dibujar_estado(self, mapa: np.ndarray,
+                       pos_x: int = 10, pos_y: int = 20) -> None:
+        """
+        Superpone el estado de evasión sobre el mapa OpenCV existente.
+        Llama a esto después de AlgoritmoSLAM.dibujar_robot_y_entorno().
+        """
+        import cv2
+
+        e = self.ultimo_estado
+        if not e:
+            return
+
+        COLOR_OK      = (0, 220, 0)
+        COLOR_PRECAU  = (0, 200, 255)
+        COLOR_PELIGRO = (0, 0, 255)
+
+        color = COLOR_PELIGRO if e["obstaculo"] else (
+                COLOR_PRECAU  if e["dist_centro"] < self.dist_precaucion
+                else COLOR_OK)
+
+        lineas = [
+            f"CMD : {e['comando']}",
+            f"IZQ : {e['dist_izq']:.2f}m",
+            f"CENT: {e['dist_centro']:.2f}m",
+            f"DER : {e['dist_der']:.2f}m",
+            e["razon"],
+        ]
+
+        # Fondo semitransparente
+        overlay = mapa.copy()
+        cv2.rectangle(overlay,
+                      (pos_x - 4,  pos_y - 16),
+                      (pos_x + 250, pos_y + len(lineas) * 18 + 4),
+                      (30, 30, 30), -1)
+        cv2.addWeighted(overlay, 0.6, mapa, 0.4, 0, mapa)
+
+        for i, linea in enumerate(lineas):
+            cv2.putText(mapa, linea,
+                        (pos_x, pos_y + i * 18),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.48, color, 1,
+                        cv2.LINE_AA)