242 lines
7.1 KiB
Python
242 lines
7.1 KiB
Python
# camera_manager.py
|
|
|
|
import depthai as dai
|
|
import cv2
|
|
import numpy as np
|
|
import time
|
|
|
|
# Parâmetros da câmera
|
|
RGB_WIDTH, RGB_HEIGHT = 640, 480
|
|
DEPTH_WIDTH, DEPTH_HEIGHT = 320, 240
|
|
FX = 440.0 # distância focal em pixels (aproximado)
|
|
BASELINE = 0.075 # distância entre câmeras estéreo (em metros)
|
|
|
|
# Variáveis globais
|
|
device = None
|
|
device_info = None
|
|
rgb_queue = None
|
|
depth_queue = None
|
|
ultimo_frame_depth = None
|
|
timestamp_ultimo_depth_frame = None
|
|
ultimo_frame_rgb = None
|
|
timestamp_ultimo_rgb_frame = None
|
|
|
|
def iniciar_camera(index=0):
|
|
global device, device_info, rgb_queue, depth_queue
|
|
|
|
print("Iniciando câmera OAK-D Lite...")
|
|
|
|
pipeline = dai.Pipeline()
|
|
|
|
# RGB
|
|
cam_rgb = pipeline.create(dai.node.ColorCamera)
|
|
cam_rgb.setPreviewSize(RGB_WIDTH, RGB_HEIGHT)
|
|
cam_rgb.setInterleaved(False)
|
|
cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB)
|
|
|
|
xout_rgb = pipeline.create(dai.node.XLinkOut)
|
|
xout_rgb.setStreamName("rgb")
|
|
cam_rgb.preview.link(xout_rgb.input)
|
|
|
|
# Profundidade
|
|
mono_left = pipeline.create(dai.node.MonoCamera)
|
|
mono_right = pipeline.create(dai.node.MonoCamera)
|
|
stereo = pipeline.create(dai.node.StereoDepth)
|
|
|
|
mono_left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
|
|
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_480_P)
|
|
mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT)
|
|
mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT)
|
|
|
|
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
|
|
|
|
mono_left.out.link(stereo.left)
|
|
mono_right.out.link(stereo.right)
|
|
|
|
xout_depth = pipeline.create(dai.node.XLinkOut)
|
|
xout_depth.setStreamName("depth")
|
|
stereo.depth.link(xout_depth.input)
|
|
|
|
# Dispositivo
|
|
available_devices = dai.Device.getAllAvailableDevices()
|
|
if index >= len(available_devices):
|
|
raise ValueError(f"Câmera de índice {index} não encontrada.")
|
|
|
|
device_info = available_devices[index]
|
|
device = dai.Device(pipeline, device_info)
|
|
|
|
rgb_queue = device.getOutputQueue(name="rgb", maxSize=1, blocking=False)
|
|
depth_queue = device.getOutputQueue(name="depth", maxSize=1, blocking=False)
|
|
|
|
print(f"Câmera iniciada: {device_info.name} (ID: {device_info.getMxId()})")
|
|
|
|
get_camera_calib()
|
|
|
|
def get_camera_calib():
|
|
global FX, BASELINE
|
|
calib = device.readCalibration()
|
|
|
|
# Obtem matriz intrínseca da câmera LEFT (com resolução padrão 640x400)
|
|
intrinsics = calib.getCameraIntrinsics(dai.CameraBoardSocket.LEFT, 640, 400)
|
|
FX = intrinsics[0][0] # fx
|
|
BASELINE = calib.getBaselineDistance() / 1000.0 # de mm → m
|
|
|
|
print(f"[CALIB] FX: {FX:.2f} px, BASELINE: {BASELINE:.4f} m")
|
|
|
|
def get_rgb_frame():
|
|
global ultimo_frame_rgb, timestamp_ultimo_rgb_frame
|
|
if rgb_queue is None:
|
|
return None, None
|
|
frame = rgb_queue.tryGet()
|
|
if frame is not None:
|
|
ultimo_frame_rgb = frame.getCvFrame()
|
|
timestamp_ultimo_rgb_frame = time.time()
|
|
return ultimo_frame_rgb, timestamp_ultimo_rgb_frame
|
|
|
|
def get_heatmap_frame():
|
|
frame = get_depth_frame()
|
|
if frame is not None:
|
|
return gerar_heatmap(frame), timestamp_ultimo_depth_frame
|
|
return None, None
|
|
|
|
def gerar_heatmap(depth_frame):
|
|
# Normaliza e aplica colormap
|
|
normalized = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX)
|
|
heatmap = cv2.applyColorMap(normalized.astype(np.uint8), cv2.COLORMAP_JET)
|
|
return heatmap
|
|
|
|
def get_depth_frame():
|
|
global ultimo_frame_depth, timestamp_ultimo_depth_frame
|
|
if depth_queue is None:
|
|
return None, None
|
|
|
|
frame = depth_queue.tryGet()
|
|
if frame is not None:
|
|
ultimo_frame_depth = frame.getFrame()
|
|
timestamp_ultimo_depth_frame = time.time()
|
|
return ultimo_frame_depth, timestamp_ultimo_depth_frame
|
|
|
|
def get_status_dispositivo():
|
|
from depthai import UsbSpeed
|
|
|
|
try:
|
|
memory = device.getDdrMemoryUsage()
|
|
memory_info = {
|
|
"used": memory.used,
|
|
"remaining": memory.remaining,
|
|
"total": memory.total
|
|
}
|
|
except:
|
|
memory_info = None
|
|
|
|
try:
|
|
temp = device.getChipTemperature()
|
|
temp_info = {
|
|
"css": temp.css,
|
|
"mss": temp.mss,
|
|
"upa": temp.upa,
|
|
"dss": temp.dss
|
|
}
|
|
except:
|
|
temp_info = None
|
|
|
|
try:
|
|
info = device.getDeviceInfo()
|
|
protocol = str(info.protocol)
|
|
except:
|
|
protocol = None
|
|
|
|
try:
|
|
bootloader = str(device.getBootloaderVersion())
|
|
except:
|
|
bootloader = None
|
|
|
|
try:
|
|
usb_speed = str(device.getUsbSpeed().name)
|
|
except:
|
|
usb_speed = None
|
|
|
|
try:
|
|
pipeline_running = device.isPipelineRunning()
|
|
except:
|
|
pipeline_running = None
|
|
|
|
try:
|
|
cameras = [sensor.name for sensor in device.getConnectedCameras()]
|
|
except:
|
|
cameras = None
|
|
|
|
return {
|
|
"id": device_info.getMxId(),
|
|
"name": device_info.name,
|
|
"state": device_info.state.name,
|
|
"usb_speed": usb_speed,
|
|
"available_camera_sensors": cameras,
|
|
"version": protocol,
|
|
"bootloader_version": bootloader,
|
|
"is_pipeline_running": pipeline_running,
|
|
"memory_usage": memory_info,
|
|
"temperature": temp_info
|
|
}
|
|
|
|
def analisar_obstaculos(velocidade=0.0, refinar=False):
|
|
from processamento.obstaculos import analisar_macro_grid, analisar_micro_grid
|
|
global timestamp_ultimo_depth_frame
|
|
|
|
frame = get_depth_frame()
|
|
|
|
if frame is None:
|
|
if ultimo_frame_depth is None:
|
|
return { "erro": "Sem frame disponível", "timestamp": None }
|
|
frame = ultimo_frame_depth
|
|
|
|
macro = analisar_macro_grid(frame, velocidade)
|
|
|
|
if macro["precisa_micro"] and refinar:
|
|
micro = analisar_micro_grid(frame)
|
|
micro["timestamp"] = timestamp_ultimo_depth_frame
|
|
return micro
|
|
|
|
macro["timestamp"] = timestamp_ultimo_depth_frame
|
|
return macro
|
|
|
|
def analisar_corredor():
|
|
from processamento.corredor import estimar_largura_corredor
|
|
global timestamp_ultimo_depth_frame # precisa garantir que fx e baseline existam
|
|
|
|
frame = get_depth_frame()
|
|
|
|
if frame is None:
|
|
if ultimo_frame_depth is None:
|
|
return { "erro": "Sem frame disponível", "timestamp": None }
|
|
frame = ultimo_frame_depth
|
|
|
|
largura_mm, pos_central, pontos_debug = estimar_largura_corredor(frame, FX, BASELINE)
|
|
|
|
return {
|
|
"largura_corredor_mm": float(largura_mm) if largura_mm is not None else None,
|
|
"posicao_central_fracao": float(pos_central) if pos_central is not None else None,
|
|
"timestamp": timestamp_ultimo_depth_frame,
|
|
"pontos_debug": [ [int(x), float(y)] for x, y in pontos_debug ]
|
|
}
|
|
|
|
def analisar_obstaculos_3d():
|
|
from processamento.visao3d import detectar_obstaculos_em_frente
|
|
global timestamp_ultimo_depth_frame
|
|
|
|
frame = get_depth_frame()
|
|
|
|
if frame is None:
|
|
if ultimo_frame_depth is None:
|
|
return { "erro": "Sem frame disponível", "timestamp": None }
|
|
frame = ultimo_frame_depth
|
|
|
|
lista = detectar_obstaculos_em_frente(frame, FX, BASELINE)
|
|
|
|
return {
|
|
"obstaculos_detectados": lista,
|
|
"timestamp": timestamp_ultimo_depth_frame
|
|
}
|
|
|
|
|