import depthai as dai import cv2 import numpy as np import time import json import sys import threading import paho.mqtt.client as mqtt import uuid from flask import Flask, Response # 🔹 Configurações MQTT mqtt_client = mqtt.Client(f"client_oak_d_lite_{uuid.uuid4()}") mqtt_client.connect("localhost", port=1883) # 🔹 Parâmetros de entrada output_folder = 'Python/Output/' mqtt_topic = sys.argv[1] # Tópico MQTT para envio dos dados porta = int(sys.argv[2]) # Porta do Flask url_rgb = sys.argv[3] # URL do vídeo RGB url_heatmap = sys.argv[4] # URL do vídeo do Heatmap max_readings = int(sys.argv[5]) camera_index = int(sys.argv[6]) # 🔹 Flask App app = Flask(__name__) # Função para calcular a moda de um array unidimensional def calc_mode(values): # Arredonda os valores para eliminar pequenas variações rounded = np.round(values, 0) vals, counts = np.unique(rounded, return_counts=True) return vals[np.argmax(counts)] # Configuração de filtros NUM_FRAMES_SMOOTH = 5 # Número de frames para suavização temporal SMOOTH_KERNEL = (5, 5) # Tamanho do kernel para suavização espacial OUTLIER_THRESHOLD = 50 # Limiar para remoção de outliers depth_buffer = [] # Buffer para armazenar os últimos frames de profundidade # Suavização Temporal def smooth_depth(depth_frame): global depth_buffer if len(depth_buffer) >= NUM_FRAMES_SMOOTH: depth_buffer.pop(0) # Remove o frame mais antigo depth_buffer.append(depth_frame) # Adiciona o novo frame return np.mean(depth_buffer, axis=0).astype(np.uint16) # Retorna a média # Suavização Espacial (Filtro Gaussiano) def gaussian_smooth(depth_frame): return cv2.GaussianBlur(depth_frame, SMOOTH_KERNEL, 0) # Filtro de Mediana para Remover Ruídos def median_filter(depth_frame): return cv2.medianBlur(depth_frame, 5) # Remover Outliers (Saltos Extremos) def remove_outliers(depth_frame): depth_median = cv2.medianBlur(depth_frame, 5) diff = np.abs(depth_frame - depth_median) depth_frame[diff > OUTLIER_THRESHOLD] = depth_median[diff > OUTLIER_THRESHOLD] return depth_frame # Aplicar filtros no depthFrame def apply_depth_filters(depth_frame): if filtro1: depth_frame = smooth_depth(depth_frame) # 1. Suavização Temporal if filtro2: depth_frame = gaussian_smooth(depth_frame) # 2. Suavização Espacial if filtro3: depth_frame = median_filter(depth_frame) # 3. Filtro de Mediana if filtro4: depth_frame = remove_outliers(depth_frame) # 4. Remoção de Outliers return depth_frame # Função para gerar o mapa de calor da profundidade def generate_heatmap(depth_frame): normalized_depth = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX) heatmap = cv2.applyColorMap(normalized_depth.astype(np.uint8), cv2.COLORMAP_JET) return heatmap # ─────────────────────────────────────────────── # Configurações da câmera e das matrizes # ─────────────────────────────────────────────── RGB_WIDTH, RGB_HEIGHT = 640, 480 WIDTH, HEIGHT = 320, 240 # Região do solo (grid A - parte inferior): ocupa 50% da altura da imagem GROUND_ROWS = 10 # Número de linhas da matriz do solo GROUND_CELLS = 10 # Número de células por linha GROUND_REGION_HEIGHT = int(HEIGHT * 0.5) # 50% da altura da imagem GROUND_TOP_SCALE = 0.5 # A linha mais distante (superior da região) terá 35% da largura total DEPTH_LIMIT = 50 # (Valor de referência para comparação, mas agora usamos calibração) # Região aérea (parte superior): ocupa o restante da imagem AIR_ROWS = 10 # Número de linhas na matriz aérea AIR_COLS = 15 # Número de colunas na matriz aérea AIR_REGION_HEIGHT = HEIGHT - GROUND_REGION_HEIGHT # Altura da região aérea # ─────────────────────────────────────────────── # Parâmetros para calibração e detecção com moda # ─────────────────────────────────────────────── NUM_CALIB_FRAMES = 100 # Número de frames para calibração NUM_DETECT_FRAMES = 10 # Número de frames para acumulação antes de calcular a moda na detecção # Variável global para armazenar o frame de profundidade atual (para uso no callback) current_depth_frame = None show_grid = False filtro1 = True filtro2 = True filtro3 = True filtro4 = True # ─────────────────────────────────────────────── # Criação do pipeline DepthAI # ─────────────────────────────────────────────── pipeline = dai.Pipeline() # Nó da câmera RGB cam_rgb = pipeline.create(dai.node.ColorCamera) cam_rgb.setPreviewSize(RGB_WIDTH, RGB_HEIGHT) cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB) cam_rgb.setInterleaved(False) xout_rgb = pipeline.create(dai.node.XLinkOut) xout_rgb.setStreamName("rgb") cam_rgb.preview.link(xout_rgb.input) # Nó de profundidade mono_left = pipeline.create(dai.node.MonoCamera) mono_right = pipeline.create(dai.node.MonoCamera) 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 = pipeline.create(dai.node.StereoDepth) stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.ROBOTICS) #stereo.initialConfig.setConfidenceThreshold(250) # Remove ruído, mantendo apenas pontos confiáveis #stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) # Usa filtro de mediana forte #stereo.setLeftRightCheck(True) # Ativa verificação para evitar erros #stereo.setExtendedDisparity(False) # Reduz ruído em distâncias curtas #stereo.setSubpixel(True) # Aumenta a precisão da profundidade 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) device_global = None selected_device_info = None camera_iniciada = False def initialize_device(): global device_global, selected_device_info, camera_iniciada # Listar todas as câmeras conectadas devices = dai.Device.getAllAvailableDevices() if len(devices) == 0: print("Nenhuma câmera OAK conectada.") return if camera_index >= len(devices): print(f"Índice da câmera ({camera_index}) inválido. Apenas {len(devices)} câmeras disponíveis.") return selected_device_info = devices[camera_index] # Seleciona a câmera correta pelo índice print(f"Usando câmera: {selected_device_info.name} (ID: {selected_device_info.mxid})") device_global = dai.Device(pipeline, selected_device_info) camera_iniciada = True # 🔹 Função para enviar vídeo via Flask def process_depth_data(): if camera_iniciada == False: initialize_device() global device_global, selected_device_info depth_queue = device_global.getOutputQueue(name="depth", maxSize=1, blocking=False) # Função para calibrar a região do solo utilizando a moda def calibrate_ground(): print("Calibrando região do solo: coletando {} frames...".format(NUM_CALIB_FRAMES)) calib_data = np.zeros((GROUND_ROWS, GROUND_CELLS, NUM_CALIB_FRAMES)) for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) calib_data[i, j, frame_idx] = np.mean(depth_frame[y_start:y_end, x_start:x_end]) ground_ref = np.zeros((GROUND_ROWS, GROUND_CELLS)) for i in range(GROUND_ROWS): for j in range(GROUND_CELLS): ground_ref[i, j] = calc_mode(calib_data[i, j, :]) print("Calibração da região do solo concluída!") return ground_ref # Função para calibrar a região aérea utilizando a moda def calibrate_air(): print("Calibrando região aérea: coletando {} frames...".format(NUM_CALIB_FRAMES)) calib_data_air = np.zeros((AIR_ROWS, AIR_COLS, NUM_CALIB_FRAMES)) row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air calib_data_air[i, j, frame_idx] = np.mean(depth_frame[y_start:y_end, x_start:x_end]) air_ref = np.zeros((AIR_ROWS, AIR_COLS)) for i in range(AIR_ROWS): for j in range(AIR_COLS): air_ref[i, j] = calc_mode(calib_data_air[i, j, :]) print("Calibração da região aérea concluída!") return air_ref # Função para calibrar a distância utilizando a moda def calibrate_distance(alvo=2500): global GROUND_REGION_HEIGHT, AIR_REGION_HEIGHT # Garantir que estamos alterando as variáveis globais NUM_LINHAS_ANALISE = 80 # Número de linhas horizontais para análise ALTURA_LINHA = HEIGHT // NUM_LINHAS_ANALISE # Altura de cada linha PROFUNDIDADE_ALVO = alvo # Profundidade alvo em cm para definir a região do solo dinamicamente media_profundidade_acumulada = np.zeros((NUM_LINHAS_ANALISE, NUM_CALIB_FRAMES)) print("Calibrando profundidade média com {} frames...".format(NUM_CALIB_FRAMES)) for frame_idx in range(NUM_CALIB_FRAMES): depth_frame = depth_queue.get().getFrame() depth_frame = apply_depth_filters(depth_frame) for i in range(NUM_LINHAS_ANALISE): y_start = HEIGHT - (i + 1) * ALTURA_LINHA y_end = y_start + ALTURA_LINHA x_start_crop = int(WIDTH * 0.0) x_end_crop = int(WIDTH * 1.0) region_values = depth_frame[y_start:y_end, x_start_crop:x_end_crop].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] if valid_values.size > 0: media_profundidade_acumulada[i, frame_idx] = np.mean(valid_values) else: media_profundidade_acumulada[i, frame_idx] = 9999 media_profundidade = np.array([calc_mode(media_profundidade_acumulada[i, :]) for i in range(NUM_LINHAS_ANALISE)]) #print("Médias de profundidade por linha (usando moda):", media_profundidade) erro_minimo = float('inf') linha_alvo = None for i in range(NUM_LINHAS_ANALISE): erro = abs(media_profundidade[i] - PROFUNDIDADE_ALVO) if erro < erro_minimo: erro_minimo = erro linha_alvo = i if linha_alvo is not None: linha_alvo += 1 GROUND_REGION_HEIGHT = HEIGHT - ((NUM_LINHAS_ANALISE - linha_alvo) * ALTURA_LINHA) else: GROUND_REGION_HEIGHT = HEIGHT // 2 AIR_REGION_HEIGHT = HEIGHT - GROUND_REGION_HEIGHT v = GROUND_REGION_HEIGHT // GROUND_ROWS print("Novo GROUND_REGION_HEIGHT:", GROUND_REGION_HEIGHT) print("Novo AIR_REGION_HEIGHT:", AIR_REGION_HEIGHT) print("Novo tamanho das linhas do solo:", v) return v # Gerar json dos dados das subdivisoes def gerar_json_subdivisoes(): data = { "x_max": WIDTH, "y_max": HEIGHT, "subdivisoes_cima": { "num_linhas": AIR_ROWS, "num_colunas": AIR_COLS, "celulas": [] }, "subdivisoes_chao": { "num_linhas": GROUND_ROWS, "num_colunas": GROUND_CELLS, "celulas": [] } } # Subdivisões da região aérea row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for i in range(AIR_ROWS): for j in range(AIR_COLS): x_start = j * cell_width_air y_start = i * row_height_air data["subdivisoes_cima"]["celulas"].append({ "linha": i, "coluna": j, "x": x_start, "y": y_start, "largura": cell_width_air, "altura": row_height_air, "profundidade_media": smoothed_air[i, j], "profundidade_calibragem": air_reference[i, j] }) # Subdivisões da região do solo row_height_ground = GROUND_REGION_HEIGHT // GROUND_ROWS for i in range(GROUND_ROWS): scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground start_x = int(WIDTH / 2 - effective_width / 2) for j in range(GROUND_CELLS): x_start = int(start_x + j * cell_width) data["subdivisoes_chao"]["celulas"].append({ "linha": i, "coluna": j, "x": x_start, "y": y_start, "largura": int(cell_width), "altura": row_height_ground, "profundidade_media": smoothed_ground[i, j], "profundidade_calibragem": ground_reference[i, j] }) return json.dumps(data, indent=4) # Calibração inicial row_height_ground = calibrate_distance() ground_reference = calibrate_ground() air_reference = calibrate_air() # Preparação para a detecção usando moda (acumula dados de alguns frames) detect_counter = 0 detect_data_ground = np.zeros((GROUND_ROWS, GROUND_CELLS, NUM_DETECT_FRAMES)) detect_data_air = np.zeros((AIR_ROWS, AIR_COLS, NUM_DETECT_FRAMES)) last_detect_ground = np.zeros((GROUND_ROWS, GROUND_CELLS)) last_detect_air = np.zeros((AIR_ROWS, AIR_COLS)) # Parâmetro para o filtro de média móvel alpha = 0.4 # ajuste entre 0 e 1 (valores menores = mais suave) # Inicialize as grids filtradas com os valores de calibração (ou com zeros, se preferir) smoothed_ground = ground_reference.copy() smoothed_air = air_reference.copy() last_time = time.time() target_fps = 30 # Limita o FPS readings = [] while True: current_time = time.time() if current_time - last_time < 1 / target_fps: time.sleep(0.01) continue last_time = current_time in_depth = depth_queue.tryGet() if in_depth is None: continue depth_frame = in_depth.getFrame() # Aplicação dos filtros em sequência depth_frame = apply_depth_filters(depth_frame) # Gera o mapa de calor heatmap = generate_heatmap(depth_frame) # Cria uma cópia do heatmap para desenhar o overlay heatmap_overlay = heatmap.copy() # ───────────────────────────────────────────── # Atualiza a grid do SOLO (região inferior) com média móvel # ───────────────────────────────────────────── for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) # Obtém os valores válidos da região region_values = depth_frame[y_start:y_end, x_start:x_end].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] # Remove valores inválidos # Calcula a média da célula e atualiza as variáveis if valid_values.size > 0: measurement = np.mean(valid_values) else: measurement = 9999 # Define um valor alto se não houver dados válidos # Atualiza a grid suavizada com a média móvel smoothed_ground[i, j] = alpha * measurement + (1 - alpha) * smoothed_ground[i, j] # Atualiza a matriz last_detect_ground com a média real da célula last_detect_ground[i, j] = measurement # ───────────────────────────────────────────── # Atualiza a grid da REGIÃO AÉREA (parte superior) com média móvel # ───────────────────────────────────────────── row_height_air = AIR_REGION_HEIGHT // AIR_ROWS cell_width_air = WIDTH // AIR_COLS for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air # Obtém os valores válidos da região region_values = depth_frame[y_start:y_end, x_start:x_end].flatten() valid_values = region_values[(region_values > 0) & (region_values < 10000)] # Remove valores inválidos # Calcula a média da célula e atualiza as variáveis if valid_values.size > 0: measurement = np.mean(valid_values) else: measurement = 9999 # Define um valor alto se não houver dados válidos # Atualiza a grid suavizada com a média móvel smoothed_air[i, j] = alpha * measurement + (1 - alpha) * smoothed_air[i, j] # Atualiza a matriz last_detect_air com a média real da célula last_detect_air[i, j] = measurement # ───────────────────────────────────────────── # Processamento da detecção: Região do SOLO # ───────────────────────────────────────────── for i in range(GROUND_ROWS): y_start = HEIGHT - GROUND_REGION_HEIGHT + i * row_height_ground y_end = y_start + row_height_ground scale = (GROUND_TOP_SCALE + (i / (GROUND_ROWS - 1)) * (1 - GROUND_TOP_SCALE)) if GROUND_ROWS > 1 else 1 effective_width = int(WIDTH * scale) cell_width = effective_width / GROUND_CELLS for j in range(GROUND_CELLS): x_start = int(WIDTH / 2 - effective_width / 2 + j * cell_width) x_end = int(x_start + cell_width) # Compara o valor suavizado com a referência calibrada if smoothed_ground[i, j] < (ground_reference[i, j] - DEPTH_LIMIT): # Obstáculo (valor menor: objeto mais próximo) cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 0, 255), -1) elif smoothed_ground[i, j] > (ground_reference[i, j] + DEPTH_LIMIT): # Erosão (valor maior: superfície rebaixada) cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 255, 255), -1) if show_grid: cv2.rectangle(heatmap, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) # ───────────────────────────────────────────── # Processamento da detecção: Região AÉREA # ───────────────────────────────────────────── for i in range(AIR_ROWS): y_start = i * row_height_air y_end = y_start + row_height_air for j in range(AIR_COLS): x_start = j * cell_width_air x_end = x_start + cell_width_air if smoothed_air[i, j] < (air_reference[i, j] - DEPTH_LIMIT): cv2.rectangle(heatmap_overlay, (x_start, y_start), (x_end, y_end), (0, 255, 0), -1) if show_grid: cv2.rectangle(heatmap, (x_start, y_start), (x_end, y_end), (255, 255, 255), 1) try: memory_usage = device_global.getDdrMemoryUsage() memory_info = { "remaining": memory_usage.remaining, "total": memory_usage.total, "used": memory_usage.used } except: memory_info = None # Se houver erro, define como None try: temp = device_global.getChipTemperature() temp_info = { "css": temp.css, "mss": temp.mss, "upa": temp.upa, "dss": temp.dss } except: temp_info = None # Se houver erro, define como None device_data = { "id": selected_device_info.getMxId(), # ID do dispositivo "name": selected_device_info.name, # Nome do dispositivo "state": selected_device_info.state.name, # Estado do dispositivo "usb_speed": str(device_global.getUsbSpeed().name) if hasattr(device_global, 'getUsbSpeed') else None, # Velocidade USB "available_camera_sensors": [sensor.name for sensor in device_global.getConnectedCameras()], # Sensores de câmera disponíveis "version": str(device_global.getDeviceInfo().protocol) if hasattr(device_global, 'getDeviceInfo') else None, # Versão do protocolo "memory_usage": memory_info, # Uso de memória DDR "temperature": temp_info, # Temperatura do chip "bootloader_version": str(device_global.getBootloaderVersion()) if hasattr(device_global, 'getBootloaderVersion') else None, # Bootloader "is_pipeline_running": device_global.isPipelineRunning() if hasattr(device_global, 'isPipelineRunning') else None # Pipeline rodando? } json_data = { 'timestamp': current_time, 'device_data': device_data, 'x_max': WIDTH, 'y_max': HEIGHT, 'subdivisoes': gerar_json_subdivisoes(), } readings.append(json_data) # Enviar apenas a cada max_readings capturas if len(readings) >= max_readings: mensagem_relevante = readings[-1] mqtt_client.publish(mqtt_topic, json.dumps(mensagem_relevante).encode('utf-8')) readings.clear() video_frame = heatmap ret, buffer = cv2.imencode('.jpg', video_frame, [cv2.IMWRITE_JPEG_QUALITY, 80]) # Reduz qualidade para 80% frame = buffer.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n') # 🔹 Função para enviar vídeo via Flask def view_rgb_video(): if camera_iniciada == False: initialize_device() global device_global, selected_device_info rgb_queue = device_global.getOutputQueue(name="rgb", maxSize=1, blocking=False) while True: in_rgb = rgb_queue.tryGet() if in_rgb is None: continue rgb_frame = in_rgb.getCvFrame() video_frame = rgb_frame ret, buffer = cv2.imencode('.jpg', video_frame, [cv2.IMWRITE_JPEG_QUALITY, 80]) # Reduz qualidade para 80% frame = buffer.tobytes() yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n') def send_script_ready(): mqtt_client.publish(mqtt_topic, "OK") def run_flask_server(): app.run(host='0.0.0.0', port=porta, threaded=True, debug=False) # 🔹 Servidores Flask para RGB e Heatmap @app.route('/' + url_rgb, methods=['GET']) def video_feed_rgb(): return Response(view_rgb_video(), mimetype='multipart/x-mixed-replace; boundary=frame') @app.route('/' + url_heatmap, methods=['GET']) def video_feed_heatmap(): return Response(process_depth_data(), mimetype='multipart/x-mixed-replace; boundary=frame') if __name__ == '__main__': mqtt_thread = threading.Thread(target=send_script_ready) mqtt_thread.start() run_flask_server()