208 lines
8.4 KiB
Python
208 lines
8.4 KiB
Python
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
|
|
|
|
# ── 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 = 500, 500
|
|
ESCALA = 200 # píxeles por metro
|
|
ORIGEN = (MAPA_W // 2, MAPA_H // 2)
|
|
|
|
# ── Objetos globales ───────────────────────────────────────────────────────────
|
|
robot = ChasisESP32(puerto='/dev/ttyAMA0')
|
|
camara = CamaraD415()
|
|
evasor = EvasorObstaculos()
|
|
ejecutando = True
|
|
|
|
# Flag de calibración en curso (evita que el autopiloto interfiera)
|
|
calibrando = False
|
|
|
|
# ── Mapa acumulado ─────────────────────────────────────────────────────────────
|
|
mapa_fondo = np.zeros((MAPA_H, MAPA_W, 3), dtype=np.uint8)
|
|
|
|
|
|
# ── 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 ───────────────────────────────────────────────────────────
|
|
def dibujar_mapa(x: float, y: float, theta: float) -> np.ndarray:
|
|
global mapa_fondo
|
|
|
|
px = int(ORIGEN[0] + x * ESCALA)
|
|
py = int(ORIGEN[1] - y * ESCALA)
|
|
px_c = max(0, min(MAPA_W - 1, px))
|
|
py_c = max(0, min(MAPA_H - 1, py))
|
|
|
|
cv2.circle(mapa_fondo, (px_c, py_c), 2, (80, 80, 80), -1)
|
|
mapa = mapa_fondo.copy()
|
|
|
|
# Cuadrícula (cada metro)
|
|
for i in range(0, MAPA_W, ESCALA):
|
|
cv2.line(mapa, (i, 0), (i, MAPA_H), (30, 30, 30), 1)
|
|
cv2.line(mapa, (0, i), (MAPA_W, i), (30, 30, 30), 1)
|
|
cv2.drawMarker(mapa, ORIGEN, (60, 60, 60), cv2.MARKER_CROSS, 12, 1)
|
|
|
|
# Robot
|
|
radio = 10
|
|
cv2.circle(mapa, (px_c, py_c), radio, (0, 200, 255), 2)
|
|
fx = int(px_c + math.cos(theta) * radio * 1.6)
|
|
fy = int(py_c - math.sin(theta) * radio * 1.6)
|
|
cv2.arrowedLine(mapa, (px_c, py_c), (fx, fy), (0, 200, 255), 2, tipLength=0.4)
|
|
|
|
# Banner de calibración en curso
|
|
if calibrando:
|
|
cv2.rectangle(mapa, (0, 0), (MAPA_W, 28), (0, 60, 120), -1)
|
|
cv2.putText(mapa, "⚙ CALIBRANDO — espera la trama K:",
|
|
(8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.52, (0, 220, 255), 1)
|
|
|
|
# HUD del evasor
|
|
evasor.dibujar_estado(mapa)
|
|
|
|
# VEL_MAX actual (para diagnóstico visual)
|
|
vmax_txt = (f"VelMax izq={robot.vel_max_izq:.0f} "
|
|
f"der={robot.vel_max_der:.0f} ticks/s")
|
|
cv2.putText(mapa, vmax_txt,
|
|
(8, MAPA_H - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.38,
|
|
(80, 160, 80), 1, cv2.LINE_AA)
|
|
|
|
# Coordenadas
|
|
cv2.putText(mapa,
|
|
f"x={x:.2f}m y={y:.2f}m θ={math.degrees(theta):.1f}°",
|
|
(8, MAPA_H - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.42,
|
|
(140, 140, 140), 1, cv2.LINE_AA)
|
|
|
|
return mapa
|
|
|
|
|
|
# ── Calibración asíncrona ─────────────────────────────────────────────────────
|
|
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")
|
|
|
|
hilo = threading.Thread(target=hilo_hardware, daemon=True)
|
|
hilo.start()
|
|
|
|
modo_autonomo = False
|
|
hilo_calib = None
|
|
|
|
try:
|
|
while True:
|
|
escaner, frame_video = camara.obtener_datos()
|
|
|
|
# Vídeo de profundidad
|
|
if frame_video is not None:
|
|
frame_video[235:245, :, :] = frame_video[240, :, :]
|
|
cv2.imshow("Vision RealSense D415", frame_video)
|
|
|
|
# Decisión del evasor
|
|
estado = evasor.decidir_movimiento(escaner)
|
|
cmd = estado["comando"]
|
|
|
|
# Autopiloto — bloqueado durante calibración
|
|
if modo_autonomo and not calibrando:
|
|
v, w = cmd_a_velocidad(cmd)
|
|
robot.enviar_velocidad(v, w)
|
|
print(f"\r🤖 [{cmd}] — {estado['razon'][:55]:<55}", end="")
|
|
elif modo_autonomo and calibrando:
|
|
# Detener mientras calibra para no interferir
|
|
robot.enviar_velocidad(0.0, 0.0)
|
|
|
|
# Mapa 2D
|
|
mapa = dibujar_mapa(robot.x, robot.y, robot.theta)
|
|
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")
|
|
modo_autonomo = True
|
|
|
|
elif tecla == ord('m'):
|
|
print("\n🛑 MODO MANUAL")
|
|
modo_autonomo = False
|
|
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)
|
|
camara.cerrar()
|
|
cv2.destroyAllWindows()
|
|
print("\n🔌 Apagado.")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main() |