213 lines
8.1 KiB
Python
213 lines
8.1 KiB
Python
"""
|
||
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)
|