""" 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)