# obstaculos.py import numpy as np import time from scipy import stats from visual_worker.enums import Direcao, TipoDeteccao, TipoRegiaoRadar LIMIAR_OBSTACULO = 1000 # mm LIMIAR_DEPRESSAO = 2500 # mm buffer_macro = None buffer_micro = None ref_macro = None ref_micro = None MACRO_ROWS = 3 MACRO_COLS = 4 MICRO_ROWS = 10 MICRO_COLS = 10 def analisar_macro_grid(depth_frame, velocidade): altura, largura = depth_frame.shape row_h = altura // MACRO_ROWS col_w = largura // MACRO_COLS LIMIAR_OBSTACULO = calcular_limiar_dinamico(velocidade) LIMIAR_DEPRESSAO = 2500 # Pode virar dinâmico depois zonas_ocupadas = [] dist_min = 9999 frontal_bloqueado = False laterais = { Direcao.ESQUERDA: False, Direcao.DIREITA: False } zona_livre = Direcao.FRENTE for i in range(MACRO_ROWS): for j in range(MACRO_COLS): y1, y2 = i * row_h, (i + 1) * row_h x1, x2 = j * col_w, (j + 1) * col_w area = depth_frame[y1:y2, x1:x2] validos = area[(area > 0) & (area < 10000)] if validos.size == 0: continue media = np.mean(validos) erro = media - ref_macro[i][j] dist_min = min(dist_min, media) tipo = TipoDeteccao.SEGURO if erro < LIMIAR_OBSTACULO: tipo = TipoDeteccao.OBSTACULO elif erro > LIMIAR_DEPRESSAO: tipo = TipoDeteccao.DEPRESSAO if tipo != TipoDeteccao.SEGURO: regiao = TipoRegiaoRadar.SOLO if i == MACRO_ROWS - 1 else TipoRegiaoRadar.AEREO zonas_ocupadas.append({ "linha": i, "coluna": j, "distancia": round(float(media) / 1000, 2), "tipo": tipo, "regiao": regiao }) if i == MACRO_ROWS - 1 and j in [1, 2]: frontal_bloqueado = True if j == 0: laterais[Direcao.ESQUERDA] = True if j == MACRO_COLS - 1: laterais[Direcao.DIREITA] = True if not zonas_ocupadas: zona_livre = Direcao.FRENTE elif all(z["coluna"] < MACRO_COLS // 2 for z in zonas_ocupadas): zona_livre = Direcao.DIREITA elif all(z["coluna"] >= MACRO_COLS // 2 for z in zonas_ocupadas): zona_livre = Direcao.ESQUERDA else: zona_livre = Direcao.PARADO return { "alerta": len(zonas_ocupadas) > 0, "frontal_bloqueado": frontal_bloqueado, "laterais": laterais, "zona_livre": zona_livre, "distancia_minima": round(float(dist_min) / 1000, 2), "detalhes": zonas_ocupadas, "precisa_micro": frontal_bloqueado } def analisar_micro_grid(depth_frame): altura, largura = depth_frame.shape row_h = altura // MICRO_ROWS col_w = largura // MICRO_COLS detalhes = [] dist_min = 9999 for i in range(MICRO_ROWS): for j in range(MICRO_COLS): y1, y2 = i * row_h, (i + 1) * row_h x1, x2 = j * col_w, (j + 1) * col_w area = depth_frame[y1:y2, x1:x2] validos = area[(area > 0) & (area < 10000)] if validos.size == 0: continue media = np.mean(validos) erro = media - ref_macro[i][j] dist_min = min(dist_min, media) tipo = TipoDeteccao.SEGURO if erro < LIMIAR_OBSTACULO: tipo = TipoDeteccao.OBSTACULO elif erro > LIMIAR_DEPRESSAO: tipo = TipoDeteccao.DEPRESSAO if tipo != TipoDeteccao.SEGURO: regiao = TipoRegiaoRadar.SOLO if i >= MICRO_ROWS * 0.6 else TipoRegiaoRadar.AEREO detalhes.append({ "linha": i, "coluna": j, "distancia": round(float(media) / 1000, 2), "tipo": tipo, "regiao": regiao }) return { "alerta": len(detalhes) > 0, "quantidade": len(detalhes), "distancia_minima": round(float(dist_min) / 1000, 2), "detalhes": detalhes } def calcular_limiar_dinamico(velocidade): # Exemplo: de 0.6m a 1.5m dependendo da velocidade return int(min(max(600 + velocidade * 800, 600), 1500)) def gerar_ref(buffer): rows = len(buffer) cols = len(buffer[0]) ref_out = np.zeros((rows, cols), dtype=np.float32) for i in range(rows): for j in range(cols): valores = buffer[i][j] if len(valores) == 0: ref_out[i][j] = 0 # ou -1 para representar célula vazia continue # Tenta usar moda moda = stats.mode(valores, keepdims=True).mode if moda.size > 0: ref_out[i][j] = moda[0] else: ref_out[i][j] = np.median(valores) return ref_out def calibrar_macro(frame): global buffer_macro altura, largura = frame.shape row_h = altura // MACRO_ROWS col_w = largura // MACRO_COLS for i in range(MACRO_ROWS): for j in range(MACRO_COLS): y1, y2 = i * row_h, (i + 1) * row_h x1, x2 = j * col_w, (j + 1) * col_w area = frame[y1:y2, x1:x2] validos = area[(area > 0) & (area < 10000)] if validos.size > 0: buffer_macro[i][j].extend(validos.tolist()) def calibrar_micro(frame): global buffer_micro altura, largura = frame.shape row_h = altura // MICRO_ROWS col_w = largura // MICRO_COLS for i in range(MICRO_ROWS): for j in range(MICRO_COLS): y1, y2 = i * row_h, (i + 1) * row_h x1, x2 = j * col_w, (j + 1) * col_w area = frame[y1:y2, x1:x2] validos = area[(area > 0) & (area < 10000)] if validos.size > 0: buffer_micro[i][j].extend(validos.tolist()) def calibrar_grades(capturar_depth_frame, n_frames=50): global buffer_macro, buffer_micro, ref_macro, ref_micro buffer_macro = [[[] for _ in range(MACRO_COLS)] for _ in range(MACRO_ROWS)] buffer_micro = [[[] for _ in range(MICRO_COLS)] for _ in range(MICRO_ROWS)] print(f"📡 Iniciando calibração de {n_frames} frames...") total_validos = 0 for i in range(n_frames): frame = capturar_depth_frame() if frame is not None: calibrar_macro(frame) calibrar_micro(frame) total_validos += 1 print(f"✔️ Frame {i+1}/{n_frames} calibrado.") else: print(f"⚠️ Frame {i+1}/{n_frames} inválido, ignorado.") time.sleep(0.03) # pausa leve pra estabilizar captura if total_validos == 0: print("❌ Nenhum frame válido recebido. Calibração cancelada.") return False ref_macro = gerar_ref(buffer_macro) ref_micro = gerar_ref(buffer_micro) print("✅ Calibração concluída com sucesso!") return True