228 lines
6.9 KiB
Python
228 lines
6.9 KiB
Python
# 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
|