Funciona todo sin modulo_imu

planificador_astar.py

@@ -0,0 +1,212 @@
+"""
+planificador_astar.py
+─────────────────────
+Planificador de rutas A* para el robot.
+Convierte el mapa SLAM (píxeles de obstáculos) en una cuadrícula
+y calcula el camino óptimo evitando obstáculos.
+
+Integración con el proyecto:
+    from planificador_astar import PlanificadorAstar
+
+    planificador = PlanificadorAstar(slam.mapa, escala=slam.escala,
+                                      centro=(slam.centro_x, slam.centro_y))
+    ruta = planificador.planificar(robot.x, robot.y,
+                                   objetivo_x, objetivo_y)
+"""
+
+import heapq
+import math
+import numpy as np
+import cv2
+from typing import Optional
+
+
+class PlanificadorAstar:
+
+    def __init__(self,
+                 mapa_slam: np.ndarray,
+                 escala: float = 50.0,
+                 centro: tuple = (400, 400),
+                 margen_obstaculo: int = 8):
+        """
+        Parameters
+        ----------
+        mapa_slam          : np.ndarray (H×W×3) — el mapa del AlgoritmoSLAM
+        escala             : píxeles por metro (mismo valor que AlgoritmoSLAM.escala)
+        centro             : (cx, cy) píxeles del origen del mapa
+        margen_obstaculo   : radio de inflado de obstáculos en píxeles
+                             (cuanto mayor, más separado pasa el robot)
+        """
+        self.escala   = escala
+        self.centro_x = centro[0]
+        self.centro_y = centro[1]
+        self.margen   = margen_obstaculo
+        self.cuadricula: Optional[np.ndarray] = None
+        self._actualizar_cuadricula(mapa_slam)
+
+    # ── API pública ──────────────────────────────────────────────────────────
+
+    def planificar(self,
+                   x_inicio: float, y_inicio: float,
+                   x_meta:   float, y_meta:   float,
+                   mapa_slam: Optional[np.ndarray] = None
+                   ) -> list[tuple[float, float]]:
+        """
+        Devuelve la lista de puntos (x_metros, y_metros) del camino,
+        desde la posición del robot hasta el objetivo.
+        Retorna [] si no existe camino.
+        """
+        if mapa_slam is not None:
+            self._actualizar_cuadricula(mapa_slam)
+
+        inicio = self._metros_a_celda(x_inicio, y_inicio)
+        meta   = self._metros_a_celda(x_meta,   y_meta)
+
+        nodos_celdas = self._buscar(inicio, meta)
+        if not nodos_celdas:
+            return []
+
+        ruta_metros = [self._celda_a_metros(c) for c in nodos_celdas]
+        return ruta_metros
+
+    def dibujar_ruta(self,
+                     mapa: np.ndarray,
+                     ruta_metros: list[tuple[float, float]],
+                     color_camino:  tuple = (0, 255, 255),
+                     color_meta:    tuple = (0, 165, 255),
+                     color_waypoint: tuple = (200, 200, 0)) -> None:
+        """Pinta la ruta calculada sobre el mapa SLAM."""
+        if not ruta_metros:
+            return
+
+        puntos_px = [self._metros_a_px(x, y) for x, y in ruta_metros]
+
+        # Línea del camino
+        for i in range(len(puntos_px) - 1):
+            cv2.line(mapa, puntos_px[i], puntos_px[i + 1], color_camino, 1)
+
+        # Waypoints
+        for p in puntos_px[1:-1]:
+            cv2.circle(mapa, p, 2, color_waypoint, -1)
+
+        # Meta
+        cv2.circle(mapa, puntos_px[-1], 6, color_meta, 2)
+        cv2.drawMarker(mapa, puntos_px[-1], color_meta,
+                       cv2.MARKER_CROSS, 12, 1)
+
+    # ── A* ───────────────────────────────────────────────────────────────────
+
+    def _buscar(self,
+                inicio: tuple[int, int],
+                meta:   tuple[int, int]
+                ) -> list[tuple[int, int]]:
+        """Núcleo del algoritmo A* sobre la cuadrícula binaria."""
+        h, w = self.cuadricula.shape
+
+        def valida(r, c):
+            return 0 <= r < h and 0 <= c < w and self.cuadricula[r, c] == 0
+
+        if not valida(*inicio) or not valida(*meta):
+            print(f"⚠️  A*: inicio o meta dentro de un obstáculo. "
+                  f"inicio={inicio} meta={meta}")
+            return []
+
+        # 8 vecinos (movimiento diagonal permitido)
+        VECINOS = [(-1,0),(1,0),(0,-1),(0,1),
+                   (-1,-1),(-1,1),(1,-1),(1,1)]
+        COSTO   = [1.0, 1.0, 1.0, 1.0,
+                   1.414, 1.414, 1.414, 1.414]
+
+        def heuristica(a, b):
+            # Distancia octile (mejor que Manhattan para movimiento diagonal)
+            dr, dc = abs(a[0]-b[0]), abs(a[1]-b[1])
+            return max(dr, dc) + (math.sqrt(2) - 1) * min(dr, dc)
+
+        # (f, g, nodo)
+        abiertos: list = []
+        heapq.heappush(abiertos, (0.0, 0.0, inicio))
+
+        g_score = {inicio: 0.0}
+        padres:  dict = {}
+        cerrados: set = set()
+
+        while abiertos:
+            f, g, actual = heapq.heappop(abiertos)
+
+            if actual == meta:
+                return self._reconstruir(padres, actual)
+
+            if actual in cerrados:
+                continue
+            cerrados.add(actual)
+
+            for (dr, dc), costo in zip(VECINOS, COSTO):
+                vecino = (actual[0] + dr, actual[1] + dc)
+                if not valida(*vecino) or vecino in cerrados:
+                    continue
+
+                # Corte diagonal: no pasar por la esquina de un obstáculo
+                if dr != 0 and dc != 0:
+                    if (self.cuadricula[actual[0]+dr, actual[1]] == 1 or
+                            self.cuadricula[actual[0], actual[1]+dc] == 1):
+                        continue
+
+                g_nuevo = g + costo
+                if g_nuevo < g_score.get(vecino, float('inf')):
+                    g_score[vecino] = g_nuevo
+                    padres[vecino]  = actual
+                    f_nuevo = g_nuevo + heuristica(vecino, meta)
+                    heapq.heappush(abiertos, (f_nuevo, g_nuevo, vecino))
+
+        print("⚠️  A*: no existe camino al objetivo")
+        return []
+
+    @staticmethod
+    def _reconstruir(padres: dict,
+                     actual: tuple) -> list[tuple]:
+        ruta = [actual]
+        while actual in padres:
+            actual = padres[actual]
+            ruta.append(actual)
+        return list(reversed(ruta))
+
+    # ── Cuadrícula ───────────────────────────────────────────────────────────
+
+    def _actualizar_cuadricula(self, mapa_slam: np.ndarray) -> None:
+        """
+        Convierte los píxeles blancos del mapa SLAM en celdas bloqueadas.
+        Aplica un margen (inflado morfológico) alrededor de cada obstáculo.
+        """
+        gris = cv2.cvtColor(mapa_slam, cv2.COLOR_BGR2GRAY)
+
+        # Píxeles cercanos al blanco → obstáculo
+        _, binario = cv2.threshold(gris, 180, 1, cv2.THRESH_BINARY)
+
+        # Inflado: el robot no puede pasar cerca de paredes
+        if self.margen > 0:
+            kernel = cv2.getStructuringElement(
+                cv2.MORPH_ELLIPSE, (self.margen*2+1, self.margen*2+1))
+            binario = cv2.dilate(binario, kernel, iterations=1)
+
+        self.cuadricula = binario.astype(np.uint8)
+
+    # ── Conversiones ─────────────────────────────────────────────────────────
+
+    def _metros_a_celda(self, x: float, y: float) -> tuple[int, int]:
+        col = int(self.centro_x + x * self.escala)
+        fil = int(self.centro_y - y * self.escala)
+        h, w = self.cuadricula.shape
+        fil = max(0, min(fil, h - 1))
+        col = max(0, min(col, w - 1))
+        return (fil, col)
+
+    def _celda_a_metros(self, celda: tuple[int, int]) -> tuple[float, float]:
+        fil, col = celda
+        x = (col - self.centro_x) / self.escala
+        y = (self.centro_y - fil) / self.escala
+        return (x, y)
+
+    def _metros_a_px(self, x: float, y: float) -> tuple[int, int]:
+        px = int(self.centro_x + x * self.escala)
+        py = int(self.centro_y - y * self.escala)
+        return (px, py)