Merge branch 'op_control' of http://zendioninc.com.br:3000/Zendion/agrobot_base into op_control

Python/raspi/multispectral_service.py

@@ -0,0 +1,146 @@
+import socket
+import json
+import numpy as np
+import base64
+import time
+
+
+class MultiSpectralService:
+    def __init__(self, host="192.168.105.6", port=5000, timeout=5):
+        self.host = host
+        self.port = port
+        self.timeout = timeout
+        self.sock = None
+        self.file = None
+
+    def __enter__(self):
+        self.connect()
+        return self
+
+    def __exit__(self, exc_type, exc, tb):
+        self.disconnect()
+
+    def connect(self):
+        if self.sock is not None:
+            return
+
+        self.sock = socket.create_connection((self.host, self.port), timeout=self.timeout)
+        self.sock.settimeout(self.timeout)
+        self.file = self.sock.makefile("r", encoding="utf-8")
+
+    def disconnect(self):
+        try:
+            if self.file:
+                self.file.close()
+        except:
+            pass
+
+        try:
+            if self.sock:
+                self.sock.close()
+        except:
+            pass
+
+        self.file = None
+        self.sock = None
+
+    def _send_command(self, payload: dict) -> dict:
+        if self.sock is None:
+            self.connect()
+
+        data = (json.dumps(payload) + "\n").encode("utf-8")
+        self.sock.sendall(data)
+
+        line = self.file.readline()
+        if not line:
+            self.disconnect()
+            raise RuntimeError("Conexão encerrada pelo servidor")
+
+        return json.loads(line.strip())
+
+    def ping(self):
+        return self._send_command({"cmd": "ping"})
+
+    def get_status(self):
+        return self._send_command({"cmd": "get_status"})
+
+    def get_config(self):
+        return self._send_command({"cmd": "get_config"})
+
+    def begin(self):
+        return self._send_command({"cmd": "begin"})
+
+    def stop(self):
+        return self._send_command({"cmd": "stop"})
+
+    def set_fps(self, fps: int):
+        return self._send_command({"cmd": "set_fps", "value": fps})
+
+    def set_jpeg_quality(self, quality: int):
+        return self._send_command({"cmd": "set_jpeg_quality", "value": quality})
+
+    def set_resolution(self, width: int, height: int):
+        return self._send_command({
+            "cmd": "set_resolution",
+            "width": width,
+            "height": height
+        })
+    
+    def capture_jpg_disk(self):
+        return self._send_command({"cmd": "capture_jpg_disk"})
+    
+    def capture_jpg_bytes(self):
+        return self._send_command({"cmd": "capture_jpg_bytes"})
+    
+    def capture_jpg_base64(self):
+        resp = self._send_command({"cmd": "capture_jpg_base64"})
+        if not resp.get("ok"):
+            raise RuntimeError(resp.get("error", "Falha ao capturar JPG"))
+        return base64.b64decode(resp["data"])
+    
+    def capture_frame_array(self):
+        t0 = time.perf_counter()
+
+        resp = self._send_command({"cmd": "capture_frame"})
+        if not resp.get("ok"):
+            raise RuntimeError(resp.get("error", "Falha ao capturar frame"))
+        
+        raw = base64.b64decode(resp["data"])
+        width = resp["width"]
+        height = resp["height"]
+        channels = resp["channels"]
+        arr = np.frombuffer(raw, dtype=np.uint8).reshape(height, width, channels)
+
+        t1 = time.perf_counter()
+
+        meta = {
+            "frame_type": resp.get("frame_type"),
+            "width": width,
+            "height": height,
+            "channels": channels,
+            "dtype": resp.get("dtype"),
+            "size": resp.get("size"),
+
+            # 👇 TELEMETRIA PI
+            "ts_pi": resp.get("ts_pi"),
+            "dt_trigger": resp.get("dt_trigger"),
+            "dt_settle": resp.get("dt_settle"),
+            "dt_capture": resp.get("dt_capture"),
+            "dt_total_pi": resp.get("dt_total_pi"),
+
+            # 👇 TELEMETRIA PC
+            "dt_total_pc": t1 - t0,
+        }
+
+        return arr, meta
+
+    def start_stream(self, host: str, port: int, fps: float):
+        return self._send_command({
+            "cmd": "start_stream",
+            "host": host,
+            "port": port,
+            "fps": fps
+        })
+
+    def stop_stream(self):
+        return self._send_command({"cmd": "stop_stream"})

Python/raspi/stream_receiver.py

@@ -0,0 +1,139 @@
+import json
+import socket
+import threading
+import time
+import lz4.frame
+import zstandard as zstd
+from numcodecs import Blosc
+import numpy as np
+
+
+class StreamReceiver:
+    def __init__(self, host="0.0.0.0", port=6001):
+        self.host = host
+        self.port = port
+
+        self._server_sock = None
+        self._client_sock = None
+        self._thread = None
+        self._running = False
+
+        self.last_frame = None
+        self.last_meta = None
+        self.last_receive_ts = None
+
+        self._zstd_d = zstd.ZstdDecompressor()
+        self._codec = Blosc(cname="lz4", clevel=1, shuffle=Blosc.SHUFFLE)
+
+    @property
+    def is_running(self):
+        return self._running
+
+    def start(self):
+        if self._running:
+            return
+
+        self._running = True
+        self._thread = threading.Thread(target=self._worker, daemon=True)
+        self._thread.start()
+
+    def stop(self):
+        self._running = False
+
+        try:
+            if self._client_sock:
+                self._client_sock.close()
+        except:
+            pass
+
+        try:
+            if self._server_sock:
+                self._server_sock.close()
+        except:
+            pass
+
+        self._client_sock = None
+        self._server_sock = None
+
+    def _recv_exact(self, sock: socket.socket, n: int) -> bytes:
+        chunks = []
+        remaining = n
+
+        while remaining > 0:
+            chunk = sock.recv(remaining)
+            if not chunk:
+                raise ConnectionError("Conexão encerrada durante recv")
+            chunks.append(chunk)
+            remaining -= len(chunk)
+
+        return b"".join(chunks)
+
+    def _worker(self):
+        try:
+            with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server:
+                server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+                server.bind((self.host, self.port))
+                server.listen(1)
+                server.settimeout(1.0)
+
+                self._server_sock = server
+                print(f"[INFO] StreamReceiver ouvindo em {self.host}:{self.port}")
+
+                while self._running:
+                    try:
+                        client, addr = server.accept()
+                    except socket.timeout:
+                        continue
+
+                    print(f"[INFO] StreamReceiver conectado por {addr}")
+                    self._client_sock = client
+
+                    with client:
+                        while self._running:
+                            header_len = int.from_bytes(self._recv_exact(client, 4), "big")
+                            header_bytes = self._recv_exact(client, header_len)
+                            header = json.loads(header_bytes.decode("utf-8"))
+
+                            payload_len = int.from_bytes(self._recv_exact(client, 4), "big")
+                            payload_comp = self._recv_exact(client, payload_len)
+                            codec = header["codec"]
+
+                            if codec == "lz4":
+                                payload = lz4.frame.decompress(payload_comp)
+                            elif codec == "zstd":
+                                payload = self._zstd_d.decompress(payload_comp)
+                            elif codec == "numcodecs":
+                                payload = self._codec.decode(payload_comp)
+                            else:
+                                raise ValueError(f"Codec não suportado: {codec}")
+                            
+                            expected = header["payload_size_raw"]
+                            if len(payload) != expected:
+                                raise ValueError(
+                                    f"Tamanho descomprimido inválido: {len(payload)} != {expected}"
+                                )
+                            
+                            height = header["height"]
+                            width = header["width"]
+                            channels = header["channels"]
+                            dtype = np.uint8 if header["dtype"] == "uint8" else None
+
+                            if dtype is None:
+                                raise RuntimeError(f"dtype não suportado: {header['dtype']}")
+
+                            frame = np.frombuffer(payload, dtype=dtype).reshape(height, width, channels)
+
+                            self.last_frame = frame
+                            self.last_meta = header
+                            self.last_receive_ts = time.perf_counter()
+
+                    print("[INFO] StreamReceiver cliente desconectado")
+                    self._client_sock = None
+
+        except Exception as e:
+            print(f"[WARN] StreamReceiver encerrado com erro: {e}")
+
+        finally:
+            self._running = False
+            self._client_sock = None
+            self._server_sock = None

Python/raspi/test_capture_raw.py

@@ -0,0 +1,19 @@
+import time
+from multispectral_service import MultiSpectralService
+
+svc = MultiSpectralService(host="192.168.105.6", port=5000)
+svc.connect()
+
+print("BEGIN:", svc.begin())
+
+tempos_pi = []
+tempos_pc = []
+
+for i in range(10):
+    frame, meta = svc.capture_frame_array()
+    tempos_pi.append(meta["dt_total_pi"])
+    tempos_pc.append(meta["dt_total_pc"])
+    print(i, meta["dt_total_pi"], meta["dt_total_pc"])
+
+print("STOP:", svc.stop())
+svc.disconnect()

Python/raspi/test_service.py

@@ -0,0 +1,29 @@
+import time
+from multispectral_service import MultiSpectralService
+
+svc = MultiSpectralService(host="192.168.105.6", port=5000)
+
+svc.connect()
+
+print("PING:", svc.ping())
+print("STATUS:", svc.get_status())
+print("SET FPS:", svc.set_fps(15))
+print("SET JPG:", svc.set_jpeg_quality(85))
+print("SET RES:", svc.set_resolution(1280, 720))
+print("BEGIN:", svc.begin())
+jpg = None
+for i in range(1, 6):  # Começa em 1 e vai até 5
+    t0 = time.time()
+    jpg = svc.capture_jpg_base64()
+    status = "OK" if jpg is not None else "Falha"
+    tempo = time.time() - t0
+    print(f"CAPTURE {i} JPG: {status}, Tempo: {tempo:.4f}")
+print("STOP:", svc.stop())
+print("CONFIG:", svc.get_config())
+print("STATUS FINAL:", svc.get_status())
+
+svc.disconnect()
+
+if jpg is not None:
+    with open("capture.jpg", "wb") as f:
+        f.write(jpg)

Python/raspi/test_stream_raw.py

@@ -0,0 +1,55 @@
+import time
+from multispectral_service import MultiSpectralService
+from stream_receiver import StreamReceiver
+
+
+STREAM_PORT = 6001
+PI_HOST = "192.168.105.6"
+PC_HOST = "192.168.105.5"
+
+
+def main():
+    receiver = StreamReceiver(host="0.0.0.0", port=STREAM_PORT)
+    receiver.start()
+
+    time.sleep(0.5)
+
+    svc = MultiSpectralService(host=PI_HOST, port=5000, timeout=10)
+    svc.connect()
+
+    try:
+        _fps = 15
+        print("SET RES:", svc.set_resolution(640, 480))
+        print("SET FPS:", svc.set_fps(_fps))
+        print("BEGIN:", svc.begin())
+
+        print("START STREAM:", svc.start_stream(PC_HOST, STREAM_PORT, fps=_fps))
+
+        t0 = time.perf_counter()
+        last_frame_id = -1
+
+        while time.perf_counter() - t0 < 5:
+            meta = receiver.last_meta
+            frame = receiver.last_frame
+            
+            if meta is not None and meta["frame_id"] != last_frame_id:
+                last_frame_id = meta["frame_id"]
+                print(meta)
+                #print(
+                #    f"frame_id={meta['frame_id']} "
+                #    f"shape={frame.shape if frame is not None else None} "
+                #    f"dt_total_pi={meta.get('dt_total_pi'):.4f}"
+                #)
+
+            time.sleep(0.02)
+
+        print("STOP STREAM:", svc.stop_stream())
+        print("STOP:", svc.stop())
+
+    finally:
+        svc.disconnect()
+        receiver.stop()
+
+
+if __name__ == "__main__":
+    main()