import time import threading import numpy as np import cv2 import math from modulo_esp32 import ChasisESP32 from modulo_camara import CamaraD415 from modulo_slam import EvasorObstaculos from modulo_imu import IMU # ── Constantes de velocidad ──────────────────────────────────────────────────── VEL_NORMAL = 0.50 # m/s en camino libre VEL_LENTA = 0.25 # m/s en precaución VEL_GIRO = 1.50 # rad/s al girar # ── Mapa 2D local ────────────────────────────────────────────────────────────── MAPA_W, MAPA_H = 700, 700 ESCALA = 150 # píxeles por metro ORIGEN = (MAPA_W // 2, MAPA_H // 2) # ── Objetos globales ─────────────────────────────────────────────────────────── imu = IMU(bus=1) # MPU6050 vía I2C robot = ChasisESP32(puerto='/dev/ttyAMA0', imu=imu) camara = CamaraD415() evasor = EvasorObstaculos() ejecutando = True # Flag de calibración en curso (evita que el autopiloto interfiera) calibrando = False # Trayectoria acumulada: lista de (px, py, velocidad_ms) para colorear por velocidad trayectoria: list = [] # Mapa de fondo estático (cuadrícula, solo se dibuja una vez y se cachea) _mapa_base: np.ndarray | None = None def _construir_base() -> np.ndarray: """Dibuja el fondo con cuadrícula y etiquetas de metros. Se llama una sola vez.""" base = np.full((MAPA_H, MAPA_W, 3), (18, 20, 26), dtype=np.uint8) # fondo casi negro # Cuadrícula menor (cada 0.5 m) paso_menor = ESCALA // 2 for i in range(0, MAPA_W, paso_menor): cv2.line(base, (i, 0), (i, MAPA_H), (35, 40, 50), 1) cv2.line(base, (0, i), (MAPA_W, i), (35, 40, 50), 1) # Cuadrícula mayor (cada 1 m) con etiqueta for i in range(0, MAPA_W, ESCALA): cv2.line(base, (i, 0), (i, MAPA_H), (55, 65, 80), 1) cv2.line(base, (0, i), (MAPA_W, i), (55, 65, 80), 1) # Etiqueta de metros en eje X metros_x = (i - ORIGEN[0]) / ESCALA if metros_x != 0: label = f"{metros_x:+.0f}m" cv2.putText(base, label, (i + 2, ORIGEN[1] - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (70, 80, 100), 1) # Etiqueta de metros en eje Y metros_y = -(i - ORIGEN[1]) / ESCALA if metros_y != 0: label = f"{metros_y:+.0f}m" cv2.putText(base, label, (ORIGEN[0] + 4, i - 2), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (70, 80, 100), 1) # Cruz de origen cv2.line(base, (ORIGEN[0], 0), (ORIGEN[0], MAPA_H), (80, 100, 130), 1) cv2.line(base, (0, ORIGEN[1]), (MAPA_W, ORIGEN[1]), (80, 100, 130), 1) cv2.drawMarker(base, ORIGEN, (100, 180, 255), cv2.MARKER_CROSS, 16, 2) cv2.putText(base, "O", (ORIGEN[0] + 6, ORIGEN[1] - 6), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (100, 180, 255), 1) return base # ── Hilo de hardware ────────────────────────────────────────────────────────── def hilo_hardware(): """ Lee odometría y tramas de control continuamente. Incluye las tramas K: (VEL_MAX) que procesa modulo_esp32 internamente. """ global ejecutando while ejecutando: robot.leer_odometria() time.sleep(0.005) # 5ms → ~200 lecturas/s, suficiente para 40ms de telemetría # ── Traducción CMD → (v, w) ──────────────────────────────────────────────────── def cmd_a_velocidad(cmd: str) -> tuple[float, float]: tabla = { EvasorObstaculos.CMD_ADELANTE : (-VEL_NORMAL, 0.0), EvasorObstaculos.CMD_ADELANTE_LENTO: (-VEL_LENTA, 0.0), EvasorObstaculos.CMD_GIRO_IZQ : (0.0, -VEL_GIRO), EvasorObstaculos.CMD_GIRO_DER : (0.0, VEL_GIRO), EvasorObstaculos.CMD_RETROCEDER : (VEL_NORMAL, 0.0), EvasorObstaculos.CMD_DETENIDO : (0.0, 0.0), } return tabla.get(cmd, (0.0, 0.0)) # ── Dibujar mapa 2D ────────────────────────────────────────────────────────────── # FOV horizontal de la RealSense D415 (profundidad) en radianes CAMARA_FOV_RAD = math.radians(69) def dibujar_mapa(x: float, y: float, theta: float, escaner: np.ndarray | None = None) -> np.ndarray: global _mapa_base if _mapa_base is None: _mapa_base = _construir_base() # Posición del robot en píxeles px = int(ORIGEN[0] + x * ESCALA) py = int(ORIGEN[1] - y * ESCALA) px_c = max(1, min(MAPA_W - 2, px)) py_c = max(1, min(MAPA_H - 2, py)) # Guardar trayectoria (solo posición, sin velocidad) trayectoria.append((px_c, py_c)) # ── Componer frame ───────────────────────────────────────────────────── mapa = _mapa_base.copy() # ── Trayectoria del robot como línea continua ────────────────────────── if len(trayectoria) >= 2: pts = np.array(trayectoria, dtype=np.int32) # Línea base (gris-azul) cv2.polylines(mapa, [pts.reshape(-1, 1, 2)], False, (60, 90, 120), 1) # Últimos 40 puntos en cian brillante para mostrar recorrido reciente reciente = pts[-40:].reshape(-1, 1, 2) cv2.polylines(mapa, [reciente], False, (0, 220, 160), 2) # ── Campo de visión de la cámara (rayos 2D) ──────────────────────────── if escaner is not None and len(escaner) == 640: N = 640 MAX_DIST_M = 4.5 # metros máximos a visualizar PASO = 5 # dibujar 1 rayo cada 5 píxeles (128 rayos) # Construir polígono del área visible (para rellenar con color) pts_fov = [(px_c, py_c)] for i in range(0, N, PASO): d = float(escaner[i]) ang = theta + (0.5 - i / (N - 1.0)) * CAMARA_FOV_RAD d_vis = min(d if 0.05 < d < MAX_DIST_M else MAX_DIST_M, MAX_DIST_M) ex = int(px_c + math.cos(ang) * d_vis * ESCALA) ey = int(py_c - math.sin(ang) * d_vis * ESCALA) pts_fov.append((max(0, min(MAPA_W-1, ex)), max(0, min(MAPA_H-1, ey)))) pts_fov.append((px_c, py_c)) # Relleno semitransparente del área de visión (verde muy oscuro) arr_fov = np.array(pts_fov, dtype=np.int32).reshape(-1, 1, 2) overlay = mapa.copy() cv2.fillPoly(overlay, [arr_fov], (0, 55, 25)) cv2.addWeighted(overlay, 0.30, mapa, 0.70, 0, mapa) # Líneas de rayos individuales coloreadas por distancia for i in range(0, N, PASO): d = float(escaner[i]) if d < 0.05: continue # lectura inválida ang = theta + (0.5 - i / (N - 1.0)) * CAMARA_FOV_RAD d_vis = min(d, MAX_DIST_M) ex = int(px_c + math.cos(ang) * d_vis * ESCALA) ey = int(py_c - math.sin(ang) * d_vis * ESCALA) ex = max(0, min(MAPA_W - 1, ex)) ey = max(0, min(MAPA_H - 1, ey)) if d < evasor.dist_peligro: color = (30, 40, 255) # rojo vivo — peligro elif d < evasor.dist_precaucion: color = (0, 165, 255) # naranja — precaución elif d < evasor.dist_libre: color = (0, 210, 120) # verde — libre else: color = (30, 100, 50) # verde oscuro — muy lejos cv2.line(mapa, (px_c, py_c), (ex, ey), color, 1) if d < evasor.dist_precaucion: # marcar hits cercanos cv2.circle(mapa, (ex, ey), 3, color, -1) # Contorno del FOV (borde blanco-azulado) cv2.polylines(mapa, [arr_fov], True, (80, 130, 180), 1) # ── Robot (encima de los rayos) ──────────────────────────────────────── radio = 12 cv2.circle(mapa, (px_c + 2, py_c + 2), radio + 2, (0, 0, 0), -1) # sombra cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), -1) cv2.circle(mapa, (px_c, py_c), radio, (0, 255, 255), 2) fx = int(px_c + math.cos(theta) * radio * 2.2) fy = int(py_c - math.sin(theta) * radio * 2.2) cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (255, 255, 255), 2, tipLength=0.35) # ── Banner de calibración ────────────────────────────────────────────── if calibrando: cv2.rectangle(mapa, (0, 0), (MAPA_W, 30), (0, 50, 110), -1) cv2.putText(mapa, " CALIBRANDO — manten robot en reposo", (8, 22), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 220, 255), 1) # ── HUD inferior ──────────────────────────────────────────────────────────── # Panel semitransparente inferior panel_y = MAPA_H - 72 overlay = mapa.copy() cv2.rectangle(overlay, (0, panel_y), (MAPA_W, MAPA_H), (10, 12, 18), -1) cv2.addWeighted(overlay, 0.80, mapa, 0.20, 0, mapa) cv2.line(mapa, (0, panel_y), (MAPA_W, panel_y), (60, 80, 110), 1) # Fila 1: pose del robot pose_txt = (f"x={x:+.2f}m y={y:+.2f}m θ={math.degrees(theta):+.1f}°") cv2.putText(mapa, pose_txt, (10, panel_y + 18), cv2.FONT_HERSHEY_SIMPLEX, 0.47, (200, 220, 255), 1, cv2.LINE_AA) # Fila 2: velocidad + IMU + modo de movimiento modo_txt = "GIRANDO" if robot.en_giro else "AVANZANDO" modo_color = (100, 160, 255) if robot.en_giro else (100, 255, 180) vel_txt = (f"V={robot.velocidad_ms * 100:.1f} cm/s " f"IMU={robot.theta_imu_deg:+.1f}° [{modo_txt}]") cv2.putText(mapa, vel_txt, (10, panel_y + 38), cv2.FONT_HERSHEY_SIMPLEX, 0.47, modo_color, 1, cv2.LINE_AA) # Fila 3: vel_max motores + estado evasor ev = evasor.ultimo_estado cmd_txt = ev.get("comando", "---") if ev else "---" vmax_txt = (f"VelMax izq={robot.vel_max_izq:.0f} der={robot.vel_max_der:.0f} t/s " f"[{cmd_txt}]") cv2.putText(mapa, vmax_txt, (10, panel_y + 56), cv2.FONT_HERSHEY_SIMPLEX, 0.38, (140, 160, 140), 1, cv2.LINE_AA) return mapa # ── Autopiloto: máquina de estados para giros de 30° ─────────────────────────── AUTO_LIBRE = "LIBRE" # avanzando según el evasor AUTO_GIRANDO = "GIRANDO" # ejecutando un paso de giro de 30° AUTO_CHECK = "CHECK" # detenido, evaluando si el camino quedó libre PASO_GIRO_RAD = math.radians(30) # tamaño del paso de giro TOLERANCIA_RAD = math.radians(4) # margen de llegada al ángulo objetivo def _diff_angulo(a: float, b: float) -> float: """Diferencia normalizada (a - b) en el rango (-π, π].""" return (a - b + math.pi) % (2 * math.pi) - math.pi def lanzar_calibracion(): """ Ejecuta la calibración en un hilo aparte para no bloquear el bucle principal. El ESP32 tarda ~2s en calibrar y responde con la trama K: automáticamente. modulo_esp32.leer_odometria() la captura en el hilo de hardware. """ global calibrando calibrando = True print("\n⚙️ Iniciando calibración (el ESP32 responderá con K: al terminar)...") robot.calibrar_pid() # Esperamos hasta 8s; si el ESP32 actualiza VEL_MAX antes, el flag # se puede bajar manualmente, pero aquí simplemente esperamos el tiempo máximo. time.sleep(8.0) calibrando = False print("\n✅ Tiempo de calibración agotado — VEL_MAX activa en el sistema.") # ── Main ─────────────────────────────────────────────────────────────────────── def main(): global ejecutando print("\n🚀 INICIANDO SISTEMA AUTÓNOMO...") print("Teclas (ventana Mapa): [a] Auto | [m] Manual | [c] Calibrar | [q] Salir") # ── Iniciar IMU (calibra bias en reposo ~1 s) ───────────────────────────── print("🧐 Calibrando IMU (mantener robot en reposo)...") imu.iniciar() print("✅ IMU lista.") hilo = threading.Thread(target=hilo_hardware, daemon=True) hilo.start() modo_autonomo = False hilo_calib = None # ── Estado del autopiloto (máquina de giros de 30°) ─────────────────────── estado_auto = AUTO_LIBRE # estado actual de la máquina theta_inicio = 0.0 # theta del IMU al iniciar el paso de giro w_giro_actual = 0.0 # dirección del giro en curso (+/-) try: while True: # Cámara: solo se usa para el evasor de obstáculos, sin mostrar video escaner, _ = camara.obtener_datos() # Decisión del evasor estado = evasor.decidir_movimiento(escaner) cmd = estado["comando"] # ── Autopiloto: máquina de estados ──────────────────────────────────────── if modo_autonomo and not calibrando: if estado_auto == AUTO_LIBRE: # ── Camino despejado: seguir al evasor ───────────────────── if cmd in (EvasorObstaculos.CMD_ADELANTE, EvasorObstaculos.CMD_ADELANTE_LENTO, EvasorObstaculos.CMD_RETROCEDER, EvasorObstaculos.CMD_DETENIDO): v, w = cmd_a_velocidad(cmd) robot.enviar_velocidad(v, w) print(f"\r🤖 [{cmd}] {estado['razon'][:50]:<50}", end="") elif cmd in (EvasorObstaculos.CMD_GIRO_IZQ, EvasorObstaculos.CMD_GIRO_DER): # ── Obstáculo detectado: iniciar primer paso de 30° ── _, w_giro_actual = cmd_a_velocidad(cmd) theta_inicio = robot.theta estado_auto = AUTO_GIRANDO robot.enviar_velocidad(0.0, w_giro_actual) print(f"\n🔄 Iniciando giro 30° (w={w_giro_actual:+.1f} rad/s)") elif estado_auto == AUTO_GIRANDO: # ── Girando: medir cuánto se ha rotado desde theta_inicio ────── girado = abs(_diff_angulo(robot.theta, theta_inicio)) if girado >= PASO_GIRO_RAD - TOLERANCIA_RAD: # Completó ~30° → detener y evaluar robot.enviar_velocidad(0.0, 0.0) estado_auto = AUTO_CHECK print(f"\n✓ Giro completado ({math.degrees(girado):.0f}°). Evaluando camino...") else: # Continuar girando robot.enviar_velocidad(0.0, w_giro_actual) print(f"\r🔄 Girando... {math.degrees(girado):.0f}° / 30°", end="") elif estado_auto == AUTO_CHECK: # ── Verificar si el camino quedó libre ──────────────────── if cmd in (EvasorObstaculos.CMD_ADELANTE, EvasorObstaculos.CMD_ADELANTE_LENTO): # ¡Camino libre! Volver a avanzar estado_auto = AUTO_LIBRE v, w = cmd_a_velocidad(cmd) robot.enviar_velocidad(v, w) print(f"\n✅ Camino libre — avanzando") elif cmd in (EvasorObstaculos.CMD_GIRO_IZQ, EvasorObstaculos.CMD_GIRO_DER): # Sigue bloqueado: otro paso de 30° en el mismo sentido theta_inicio = robot.theta estado_auto = AUTO_GIRANDO robot.enviar_velocidad(0.0, w_giro_actual) print(f"\n🔄 Sigue bloqueado — otro paso de 30°") else: # RETROCEDER o DETENIDO estado_auto = AUTO_LIBRE v, w = cmd_a_velocidad(cmd) robot.enviar_velocidad(v, w) elif modo_autonomo and calibrando: robot.enviar_velocidad(0.0, 0.0) estado_auto = AUTO_LIBRE # Mapa 2D mapa = dibujar_mapa(robot.x, robot.y, robot.theta, escaner) cv2.imshow("Mapa SLAM", mapa) # Teclado tecla = cv2.waitKey(1) & 0xFF if tecla == ord('a'): if calibrando: print("\n⚠️ Calibración en curso — espera antes de activar autopiloto.") else: print("\n🤖 AUTOPILOTO ON") estado_auto = AUTO_LIBRE theta_inicio = robot.theta w_giro_actual = 0.0 modo_autonomo = True elif tecla == ord('m'): print("\n🛑 MODO MANUAL") modo_autonomo = False estado_auto = AUTO_LIBRE robot.enviar_velocidad(0.0, 0.0) elif tecla == ord('c'): if calibrando: print("\n⚠️ Ya hay una calibración en curso.") else: # Detener robot primero modo_autonomo = False robot.enviar_velocidad(0.0, 0.0) time.sleep(0.2) # Lanzar calibración en hilo para no bloquear la UI hilo_calib = threading.Thread(target=lanzar_calibracion, daemon=True) hilo_calib.start() elif tecla == ord('q'): break except KeyboardInterrupt: pass finally: ejecutando = False hilo.join(timeout=1.0) if hilo_calib and hilo_calib.is_alive(): hilo_calib.join(timeout=2.0) robot.enviar_velocidad(0.0, 0.0) imu.detener() camara.cerrar() cv2.destroyAllWindows() print("\n🔌 Apagado.") if __name__ == "__main__": main()