agrobot_base/Python/raspi/cam_3/pi/state.py

683 lines
23 KiB
Python

from dataclasses import dataclass, field, asdict
from typing import List, Optional, Dict, Any
@dataclass
class CameraSpec:
id: str
index: int
role: str = "generic" # re | nir | rgb | generic
interface: str = "CSI" # CSI | USB
connected: bool = False
enabled: bool = True
available: bool = False
width: int = 640
height: int = 480
bayer_pattern: str = "GBRG"
bit_depth: int = 10
model: Optional[str] = None
serial: Optional[str] = None
# Novos campos
device_path: Optional[str] = None # ex: /dev/video0
usb_backend: str = "V4L2" # V4L2 | DEFAULT
preferred: bool = True # ajuda em resolução de conflitos
def to_dict(self) -> Dict[str, Any]:
return asdict(self)
@dataclass
class PayloadSpec:
frame_type: str = "RAW_BRUTO" # RAW_BRUTO | RGB | MULTISPEC
dtype: str = "uint8" # uint8 | uint16 | float32
layout: str = "HW" # HW | CHW | MULTI_HW
channels: int = 1
channel_names: List[str] = field(default_factory=lambda: ["BAYER"])
width: int = 640
height: int = 480
sources: List[str] = field(default_factory=lambda: ["cam2"])
complete: bool = False
def to_dict(self) -> Dict[str, Any]:
return asdict(self)
class ModuleState:
def __init__(self):
self.module_name = "multispectral"
self.version = "0.3.0"
self.payload_format_version = 3
self.status = "idle"
self.status_detail = None
self.last_error = None
self.last_command = None
self.initialized = False
self.streaming = False
# Estratégia do módulo
# AUTO = decide com base nas câmeras disponíveis
# SINGLE = usa apenas uma câmera relevante para o frame_type
# DOUBLE = exige a camera RGB + 1 camera CSI
# TRIPLE = exige o conjunto completo
self.capture_mode = "AUTO" # AUTO | SINGLE | DOUBLE | TRIPLE
self.detected_mode = "NONE" # NONE | PARTIAL | RGB_ONLY | MS_ONLY | DOUBLE | TRIPLE
self.multi_camera_enabled = True
self.max_cameras = 3
# Configuração global base
self.fps = 10
self.jpeg_quality = 90
# Frame desejado de saída
self.frame_type = "RAW_BRUTO" # RAW_BRUTO | RGB | MULTISPEC
self.output_dtype = "uint8" # uint8 | uint16 | float32
# Lista fixa das câmeras do módulo
# cam0 = RE (CSI)
# cam1 = NIR (CSI)
# cam2 = RGB (USB)
self.cameras: List[CameraSpec] = [
CameraSpec(
id="cam0",
index=0,
role="re",
interface="CSI",
bayer_pattern="GBRG",
bit_depth=10,
),
CameraSpec(
id="cam1",
index=1,
role="nir",
interface="CSI",
bayer_pattern="GBRG",
bit_depth=10,
),
CameraSpec(
id="cam2",
index=2,
role="rgb",
interface="USB",
bayer_pattern="GBRG", # pode ficar por compatibilidade, embora USB RGB não use isso
bit_depth=8, # melhor refletir a realidade da USB
device_path="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN0001-video-index0",
usb_backend="V4L2",
),
]
# Resumo dinâmico da topologia
self.camera_count_detected = 0
self.camera_count_active = 0
self.active_camera_ids: List[str] = []
self.re_camera_id = "cam0"
self.nir_camera_id = "cam1"
self.rgb_camera_id = "cam2"
self.has_re = False
self.has_nir = False
self.has_rgb = False
self.has_multispec_pair = False
self.has_full_multispec = False
# Spec do payload atual
self.payload = PayloadSpec()
# Trigger
# Idealmente aplicado às duas CSI (RE e NIR)
self.trigger_enabled = True
self.trigger_pin = 18
self.trigger_active_high = True
self.trigger_pulse_ms = 5.0
self.trigger_settle_delay_ms = 0.0
# Streaming
self.stream_host = None
self.stream_port = None
self.stream_fps = None
self.stream_frame_id = 0
self.stream_frame_id_sent = 0
# Codec
self.codec_family = "none" # numcodecs
self.codec_name = "blosc"
self.codec_params = {
"cname": "zstd",
"clevel": 3,
"shuffle": "BITSHUFFLE",
}
# Controle das câmeras
self.ae_enable = True
self.awb_enable = True
self.exposure_time_us = 15000
self.analogue_gain = 1.0
self.colour_gains = [1.0, 1.0]
self.frame_duration_limits = None
# Inicialização
self.refresh_topology()
self.update_payload_spec()
# =========================================================
# Helpers de câmera
# =========================================================
def get_camera(self, cam_id: str) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.id == cam_id:
return cam
return None
def get_camera_by_index(self, index: int) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.index == index:
return cam
return None
def get_camera_by_role(self, role: str) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.role == role:
return cam
return None
def get_camera_by_device_path(self, device_path: str) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.device_path == device_path:
return cam
return None
def get_active_camera_by_role(self, role: str) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.role == role and cam.connected and cam.enabled:
return cam
return None
def get_enabled_camera_by_role(self, role: str) -> Optional[CameraSpec]:
for cam in self.cameras:
if cam.role == role and cam.enabled:
return cam
return None
def get_bootstrap_camera_ids_for_frame_type(self) -> List[str]:
def enabled(role: str):
return self.get_enabled_camera_by_role(role)
rgb_cam = enabled("rgb")
re_cam = enabled("re")
nir_cam = enabled("nir")
if self.frame_type == "RGB":
return [rgb_cam.id] if rgb_cam else []
if self.frame_type in ("RAW_BRUTO", "MULTISPEC"):
if self.capture_mode == "TRIPLE":
ids = []
if rgb_cam:
ids.append(rgb_cam.id)
if re_cam:
ids.append(re_cam.id)
if nir_cam:
ids.append(nir_cam.id)
return ids
if self.capture_mode == "DOUBLE":
ids = []
if rgb_cam:
ids.append(rgb_cam.id)
if re_cam:
ids.append(re_cam.id)
elif nir_cam:
ids.append(nir_cam.id)
return ids
if self.capture_mode == "SINGLE":
for cam in (rgb_cam, re_cam, nir_cam):
if cam:
return [cam.id]
return []
if self.capture_mode == "AUTO":
ids = []
if rgb_cam:
ids.append(rgb_cam.id)
if re_cam:
ids.append(re_cam.id)
if nir_cam:
ids.append(nir_cam.id)
if ids:
return ids
return []
return []
return []
def get_required_camera_ids_for_frame_type(self) -> List[str]:
def activated(role: str):
return self.get_active_camera_by_role(role)
def enabled(role: str):
return self.get_enabled_camera_by_role(role)
rgb_cam = activated("rgb")
re_cam = activated("re")
nir_cam = activated("nir")
resolved = self.resolve_capture_mode()
if self.frame_type == "RGB":
return [rgb_cam.id] if rgb_cam else []
if self.frame_type == "MULTISPEC":
if resolved == "TRIPLE":
if rgb_cam and re_cam and nir_cam:
return [rgb_cam.id, re_cam.id, nir_cam.id]
return []
if resolved == "DOUBLE":
if rgb_cam and re_cam:
return [rgb_cam.id, re_cam.id]
if rgb_cam and nir_cam:
return [rgb_cam.id, nir_cam.id]
return []
# MULTISPEC não faz sentido com SINGLE
return []
if self.frame_type == "RAW_BRUTO":
if resolved == "TRIPLE":
return [cam.id for cam in self.cameras if cam.enabled]
if resolved == "DOUBLE":
ids = []
if rgb_cam:
ids.append(rgb_cam.id)
if re_cam:
ids.append(re_cam.id)
elif nir_cam:
ids.append(nir_cam.id)
return ids
if resolved == "SINGLE":
for cam in (rgb_cam, re_cam, nir_cam):
if cam:
return [cam.id]
return []
return []
return []
def set_camera_device_path(self, index: int, device_path: Optional[str]) -> None:
cam = self.get_camera_by_index(index)
if cam is None:
raise ValueError(f"Câmera de índice {index} não existe.")
cam.device_path = device_path
def set_camera_usb_backend(self, index: int, usb_backend: str) -> None:
cam = self.get_camera_by_index(index)
if cam is None:
raise ValueError(f"Câmera de índice {index} não existe.")
cam.usb_backend = usb_backend
# =========================================================
# Topologia / câmeras
# =========================================================
def set_camera_connected(
self,
index: int,
connected: bool,
*,
width: Optional[int] = None,
height: Optional[int] = None,
bayer_pattern: Optional[str] = None,
bit_depth: Optional[int] = None,
model: Optional[str] = None,
serial: Optional[str] = None,
) -> None:
cam = self.get_camera_by_index(index)
if cam is None:
raise ValueError(f"Câmera de índice {index} não existe.")
cam.connected = connected
cam.available = connected and cam.enabled
if width is not None:
cam.width = width
if height is not None:
cam.height = height
if bayer_pattern is not None:
cam.bayer_pattern = bayer_pattern
if bit_depth is not None:
cam.bit_depth = bit_depth
if model is not None:
cam.model = model
if serial is not None:
cam.serial = serial
self.refresh_topology()
self.update_payload_spec()
def set_camera_enabled(self, index: int, enabled: bool) -> None:
cam = self.get_camera_by_index(index)
if cam is None:
raise ValueError(f"Câmera de índice {index} não existe.")
cam.enabled = enabled
cam.available = cam.connected and cam.enabled
self.refresh_topology()
self.update_payload_spec()
def refresh_topology(self) -> None:
detected = [cam for cam in self.cameras if cam.connected]
active = [cam for cam in self.cameras if cam.connected and cam.enabled]
self.camera_count_detected = len(detected)
self.camera_count_active = len(active)
self.active_camera_ids = [cam.id for cam in active]
re_cam = self.get_active_camera_by_role("re")
nir_cam = self.get_active_camera_by_role("nir")
rgb_cam = self.get_active_camera_by_role("rgb")
self.has_re = re_cam is not None
self.has_nir = nir_cam is not None
self.has_rgb = rgb_cam is not None
self.has_multispec_pair = self.has_re and self.has_nir
self.has_full_multispec = self.has_re and self.has_nir and self.has_rgb
if self.has_full_multispec and self.multi_camera_enabled:
self.detected_mode = "TRIPLE"
elif self.has_rgb and (self.has_re or self.has_nir):
self.detected_mode = "DOUBLE"
elif self.has_multispec_pair and not self.has_rgb:
self.detected_mode = "MS_ONLY"
elif self.has_rgb and not self.has_multispec_pair:
self.detected_mode = "RGB_ONLY"
elif self.camera_count_active > 0:
self.detected_mode = "PARTIAL"
else:
self.detected_mode = "NONE"
def resolve_capture_mode(self) -> str:
connected_re = self.get_active_camera_by_role("re") is not None
connected_nir = self.get_active_camera_by_role("nir") is not None
connected_rgb = self.get_active_camera_by_role("rgb") is not None
if self.capture_mode == "AUTO":
if connected_rgb and connected_re and connected_nir and self.multi_camera_enabled:
return "TRIPLE"
if connected_rgb and (connected_re or connected_nir):
return "DOUBLE"
if connected_re and connected_nir:
return "DOUBLE" if self.frame_type == "RAW_BRUTO" else "NONE"
if connected_rgb or connected_re or connected_nir:
return "SINGLE"
return "NONE"
if self.capture_mode == "TRIPLE":
return "TRIPLE" if (connected_rgb and connected_re and connected_nir and self.multi_camera_enabled) else "NONE"
if self.capture_mode == "DOUBLE":
if connected_rgb and (connected_re or connected_nir):
return "DOUBLE"
if self.frame_type == "RAW_BRUTO" and connected_re and connected_nir:
return "DOUBLE"
return "NONE"
if self.capture_mode == "SINGLE":
return "SINGLE" if (connected_rgb or connected_re or connected_nir) else "NONE"
return "NONE"
# =========================================================
# Payload
# =========================================================
def update_payload_spec(self) -> None:
resolved_mode = self.resolve_capture_mode()
if resolved_mode == "NONE":
self.payload = PayloadSpec(
frame_type=self.frame_type,
dtype=self.output_dtype,
layout="HW",
channels=0,
channel_names=[],
width=0,
height=0,
sources=[],
complete=False,
)
return
rgb_cam = self.get_active_camera_by_role("rgb")
re_cam = self.get_active_camera_by_role("re")
nir_cam = self.get_active_camera_by_role("nir")
if self.frame_type == "RAW_BRUTO":
sources = self.get_required_camera_ids_for_frame_type()
if len(sources) == 1:
cam = self.get_camera(sources[0])
self.payload = PayloadSpec(
frame_type="RAW_BRUTO",
dtype=self.output_dtype,
layout="HW",
channels=1,
channel_names=["BAYER"],
width=cam.width if cam else 0,
height=cam.height if cam else 0,
sources=sources,
complete=(cam is not None),
)
return
if len(sources) >= 2:
# para 3 câmeras usamos MULTI_HW
# cada origem será empacotada separadamente
ref_cam = self.get_camera(sources[0]) if sources else None
self.payload = PayloadSpec(
frame_type="RAW_BRUTO",
dtype=self.output_dtype,
layout="MULTI_HW",
channels=len(sources),
channel_names=[f"BAYER_{cam_id.upper()}" for cam_id in sources],
width=ref_cam.width if ref_cam else 0,
height=ref_cam.height if ref_cam else 0,
sources=sources,
complete=True,
)
return
self.payload = PayloadSpec(
frame_type="RAW_BRUTO",
dtype=self.output_dtype,
layout="HW",
channels=0,
channel_names=[],
width=0,
height=0,
sources=[],
complete=False,
)
return
if self.frame_type == "RGB":
if rgb_cam is None:
self.payload = PayloadSpec(
frame_type="RGB",
dtype=self.output_dtype,
layout="CHW",
channels=0,
channel_names=[],
width=0,
height=0,
sources=[],
complete=False,
)
return
self.payload = PayloadSpec(
frame_type="RGB",
dtype=self.output_dtype,
layout="CHW",
channels=3,
channel_names=["R", "G", "B"],
width=rgb_cam.width,
height=rgb_cam.height,
sources=[rgb_cam.id],
complete=True,
)
return
if self.frame_type == "MULTISPEC":
resolved = self.resolve_capture_mode()
if resolved == "TRIPLE" and rgb_cam and re_cam and nir_cam:
out_w = min(rgb_cam.width, re_cam.width // 2, nir_cam.width // 2)
out_h = min(rgb_cam.height, re_cam.height, nir_cam.height)
self.payload = PayloadSpec(
frame_type="MULTISPEC",
dtype=self.output_dtype,
layout="CHW",
channels=5,
channel_names=["R", "G", "B", "RE", "NIR"],
width=out_w,
height=out_h,
sources=[rgb_cam.id, re_cam.id, nir_cam.id],
complete=True,
)
return
if resolved == "DOUBLE" and rgb_cam and (re_cam or nir_cam):
ms_cam = re_cam if re_cam is not None else nir_cam
ms_name = "RE" if re_cam is not None else "NIR"
out_w = min(rgb_cam.width, ms_cam.width // 2)
out_h = min(rgb_cam.height, ms_cam.height)
self.payload = PayloadSpec(
frame_type="MULTISPEC",
dtype=self.output_dtype,
layout="CHW",
channels=4,
channel_names=["R", "G", "B", ms_name],
width=out_w,
height=out_h,
sources=[rgb_cam.id, ms_cam.id],
complete=True,
)
return
self.payload = PayloadSpec(
frame_type="MULTISPEC",
dtype=self.output_dtype,
layout="CHW",
channels=0,
channel_names=[],
width=0,
height=0,
sources=[],
complete=False,
)
return
raise ValueError(f"frame_type inválido: {self.frame_type}")
# =========================================================
# Estado / erro
# =========================================================
def clear_error(self) -> None:
self.last_error = None
if self.status == "error":
self.status = "idle"
self.status_detail = None
def set_error(self, error: str) -> None:
self.last_error = str(error)
self.status = "error"
self.status_detail = str(error)
# =========================================================
# Serialização
# =========================================================
def to_dict(self) -> Dict[str, Any]:
return {
"module": self.module_name,
"version": self.version,
"status": self.status,
"status_detail": self.status_detail,
"last_error": self.last_error,
"last_command": self.last_command,
"initialized": self.initialized,
"streaming": self.streaming,
"capture_mode": self.capture_mode,
"detected_mode": self.detected_mode,
"multi_camera_enabled": self.multi_camera_enabled,
"max_cameras": self.max_cameras,
"camera_count_detected": self.camera_count_detected,
"camera_count_active": self.camera_count_active,
"active_camera_ids": self.active_camera_ids,
"has_re": self.has_re,
"has_nir": self.has_nir,
"has_rgb": self.has_rgb,
"has_multispec_pair": self.has_multispec_pair,
"has_full_multispec": self.has_full_multispec,
"re_camera_id": self.re_camera_id,
"nir_camera_id": self.nir_camera_id,
"rgb_camera_id": self.rgb_camera_id,
"cameras": [cam.to_dict() for cam in self.cameras],
"fps": self.fps,
"jpeg_quality": self.jpeg_quality,
"trigger_enabled": self.trigger_enabled,
"trigger_pin": self.trigger_pin,
"trigger_active_high": self.trigger_active_high,
"trigger_pulse_ms": self.trigger_pulse_ms,
"trigger_settle_delay_ms": self.trigger_settle_delay_ms,
"stream_host": self.stream_host,
"stream_port": self.stream_port,
"stream_fps": self.stream_fps,
"stream_frame_id": self.stream_frame_id,
"stream_frame_id_sent": self.stream_frame_id_sent,
"codec_family": self.codec_family,
"codec_name": self.codec_name,
"codec_params": self.codec_params,
"ae_enable": self.ae_enable,
"awb_enable": self.awb_enable,
"exposure_time_us": self.exposure_time_us,
"analogue_gain": self.analogue_gain,
"colour_gains": self.colour_gains,
"frame_duration_limits": self.frame_duration_limits,
"payload_format_version": self.payload_format_version,
"frame_type": self.frame_type,
"output_dtype": self.output_dtype,
"payload": self.payload.to_dict(),
}