115 lines
4.4 KiB
Python
115 lines
4.4 KiB
Python
import argparse
|
|
import base64
|
|
import json
|
|
import socket
|
|
import time
|
|
|
|
import cv2
|
|
import numpy as np
|
|
|
|
import flywms_navigation as nav
|
|
|
|
|
|
def parse_args():
|
|
pre = argparse.ArgumentParser(add_help=False)
|
|
pre.add_argument("--config", default=nav.DEFAULT_CONFIG_PATH, help="File configurazione INI")
|
|
pre_args, _ = pre.parse_known_args()
|
|
defaults = nav.load_navigation_config(pre_args.config)
|
|
|
|
ap = argparse.ArgumentParser(parents=[pre])
|
|
ap.add_argument("--host", default=defaults["observer_host"], help="Host publisher observer")
|
|
ap.add_argument("--port", type=int, default=defaults["observer_port"], help="Porta publisher observer")
|
|
ap.add_argument("--reconnect-sec", type=float, default=1.0, help="Attesa tra tentativi di reconnessione")
|
|
return ap.parse_args()
|
|
|
|
|
|
def decode_preview(message: dict[str, object]) -> np.ndarray | None:
|
|
image_b64 = message.get("image_b64")
|
|
if not isinstance(image_b64, str):
|
|
return None
|
|
try:
|
|
raw = base64.b64decode(image_b64.encode("ascii"))
|
|
array = np.frombuffer(raw, dtype=np.uint8)
|
|
return cv2.imdecode(array, cv2.IMREAD_COLOR)
|
|
except (ValueError, cv2.error):
|
|
return None
|
|
|
|
|
|
def main() -> int:
|
|
args = parse_args()
|
|
nav.log(f"[OBS] avvio observer {args.host}:{args.port}")
|
|
|
|
latest_navigate: np.ndarray | None = None
|
|
latest_snapshot: np.ndarray | None = None
|
|
latest_label: np.ndarray | None = None
|
|
telemetry: dict[str, object] = {
|
|
"command_lines": ["Observer in attesa dati"],
|
|
"motion_text": "MOTO: n/d",
|
|
}
|
|
|
|
cv2.namedWindow("flywms observer", cv2.WINDOW_NORMAL)
|
|
cv2.namedWindow("flywms observer comandi", cv2.WINDOW_NORMAL)
|
|
cv2.namedWindow("flywms observer snapshot", cv2.WINDOW_NORMAL)
|
|
cv2.namedWindow("flywms observer etichetta", cv2.WINDOW_NORMAL)
|
|
|
|
while True:
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
try:
|
|
sock.connect((args.host, args.port))
|
|
nav.log("[OBS] connesso al core")
|
|
fileobj = sock.makefile("r", encoding="utf-8", newline="\n")
|
|
while True:
|
|
line = fileobj.readline()
|
|
if not line:
|
|
raise ConnectionError("publisher disconnesso")
|
|
message = json.loads(line)
|
|
msg_type = message.get("type")
|
|
if msg_type == "telemetry":
|
|
telemetry = message
|
|
elif msg_type == "preview":
|
|
frame = decode_preview(message)
|
|
stream = message.get("stream")
|
|
if frame is None:
|
|
continue
|
|
if stream == "navigate":
|
|
latest_navigate = frame
|
|
elif stream == "snapshot":
|
|
latest_snapshot = frame
|
|
elif stream == "label":
|
|
latest_label = frame
|
|
|
|
if latest_navigate is not None:
|
|
cv2.imshow("flywms observer", latest_navigate)
|
|
if latest_snapshot is not None:
|
|
cv2.imshow("flywms observer snapshot", latest_snapshot)
|
|
if latest_label is not None:
|
|
cv2.imshow("flywms observer etichetta", latest_label)
|
|
command_lines = telemetry.get("command_lines") or ["Observer attivo"]
|
|
if not isinstance(command_lines, list):
|
|
command_lines = [str(command_lines)]
|
|
motion_text = str(telemetry.get("motion_text") or "MOTO: n/d")
|
|
panel = nav.draw_commands_window([str(item) for item in command_lines], motion_text)
|
|
cv2.imshow("flywms observer comandi", panel)
|
|
key = cv2.waitKey(1) & 0xFF
|
|
if key in (27, ord("q")):
|
|
nav.log("[OBS] terminato da tastiera")
|
|
return 0
|
|
except (ConnectionError, OSError, json.JSONDecodeError) as exc:
|
|
nav.log(f"[OBS] connessione persa o non disponibile: {exc}")
|
|
wait_deadline = time.perf_counter() + max(0.2, args.reconnect_sec)
|
|
while time.perf_counter() < wait_deadline:
|
|
key = cv2.waitKey(50) & 0xFF
|
|
if key in (27, ord("q")):
|
|
nav.log("[OBS] terminato da tastiera")
|
|
return 0
|
|
continue
|
|
finally:
|
|
try:
|
|
sock.close()
|
|
except OSError:
|
|
pass
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|