agrobot_base/Python/OAK/OAK-D-Lite_depthData.py

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, request

# 🔹 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 de profundidade
porta = int(sys.argv[2])  # Porta do Flask
url = sys.argv[3]  # URL do vídeo
max_readings = int(sys.argv[4])

# 🔹 Flask App
app = Flask(__name__)

# 🔹 Criando o pipeline
pipeline = dai.Pipeline()

width, height = 1280, 720

# 📷 Câmera RGB
cam_rgb = pipeline.create(dai.node.ColorCamera)
cam_rgb.setPreviewSize(width, height)
cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB)
cam_rgb.setInterleaved(False)

# Criar XLinkOut para saída RGB
xout_rgb = pipeline.create(dai.node.XLinkOut)
xout_rgb.setStreamName("rgb")
cam_rgb.preview.link(xout_rgb.input)

# 🔹 Câmera de 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_400_P)
mono_right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)

mono_left.setBoardSocket(dai.CameraBoardSocket.LEFT)
mono_right.setBoardSocket(dai.CameraBoardSocket.RIGHT)

# Configuração do StereoDepth
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.setLeftRightCheck(True)
stereo.setSubpixel(True)

mono_left.out.link(stereo.left)
mono_right.out.link(stereo.right)

# Criar XLinkOut para profundidade
xout_depth = pipeline.create(dai.node.XLinkOut)
xout_depth.setStreamName("depth")
stereo.depth.link(xout_depth.input)


# 🔹 Função para enviar vídeo via Flask
def generate_video(camera_index):
    # 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})")

    with dai.Device(pipeline, selected_device_info) as device:  # 🔹 Agora usa a câmera correta
        rgb_queue = device.getOutputQueue(name="rgb", maxSize=1, blocking=True)
        depth_queue = device.getOutputQueue(name="depth", maxSize=1, blocking=True)

        readings = []
        rgb_frame = None
        depth_frame = None

        while True:
            timestamp = time.time()
            
            # Pega os dois frames ao mesmo tempo, para evitar desincronização
            in_rgb = rgb_queue.tryGet()
            in_depth = depth_queue.tryGet()

            if in_rgb is not None and in_depth is not None:
                rgb_frame = in_rgb.getCvFrame()
                depth_frame = in_depth.getFrame()

            # 🔹 Verifica se depth_frame está vazio antes de processar
            if depth_frame is None or depth_frame.size == 0:
                print("⚠️ Frame de profundidade inválido. Pulando esta iteração.")
                continue

            # Normaliza a profundidade para visualização (mapa de calor)
            depth_visual = cv2.normalize(depth_frame, None, 0, 255, cv2.NORM_MINMAX, dtype=cv2.CV_8U)
            depth_visual = cv2.applyColorMap(depth_visual, cv2.COLORMAP_JET)

            # 📡 Enviar dados via MQTT (Apenas a matriz de profundidade reduzida)
            depth_data = depth_frame.tolist()  # Converte a matriz para lista JSON
            depth_small = cv2.resize(depth_frame, (width // 2, height // 2))  # Reduz para metade
            depth_data = depth_small.tolist()
            
            try:
                memory_usage = device.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.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.getUsbSpeed().name) if hasattr(device, 'getUsbSpeed') else None,  # Velocidade USB
                "available_camera_sensors": [sensor.name for sensor in device.getConnectedCameras()],  # Sensores de câmera disponíveis
                "version": str(device.getDeviceInfo().protocol) if hasattr(device, '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.getBootloaderVersion()) if hasattr(device, 'getBootloaderVersion') else None,  # Bootloader
                "is_pipeline_running": device.isPipelineRunning() if hasattr(device, 'isPipelineRunning') else None  # Pipeline rodando?
            }


            json_data = {
                'timestamp': timestamp,
                'device_data': device_data,
                'x_max': width, 
                'y_max': height, 
                'depth_data': depth_data,
            }

            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()


            frame = rgb_frame.getCvFrame()
            ret, buffer = cv2.imencode('.jpg', 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)

@app.route('/' + url, methods=['GET'])
def video_feed():
    camera_index = int(request.args.get('camera_index'))
    return Response(generate_video(camera_index), mimetype='multipart/x-mixed-replace; boundary=frame')

if __name__ == '__main__':
    mqtt_thread = threading.Thread(target=send_script_ready)
    mqtt_thread.start()
    run_flask_server()