196 lines
9.6 KiB
Python
196 lines
9.6 KiB
Python
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) |