import argparse import base64 import configparser import json import queue import socket import sys import threading import time import uuid from dataclasses import dataclass, field from pathlib import Path from urllib import request, error import cv2 import numpy as np DEFAULT_CONFIG_PATH = "flywms_navigation.ini" CUDA_MIN_PIXELS = 640 * 360 def opencv_cuda_available() -> bool: try: return hasattr(cv2, "cuda") and cv2.cuda.getCudaEnabledDeviceCount() > 0 except cv2.error: return False OPENCV_CUDA_AVAILABLE = opencv_cuda_available() def cuda_resize( image: np.ndarray, size: tuple[int, int], interpolation: int = cv2.INTER_LINEAR, min_pixels: int = CUDA_MIN_PIXELS, ) -> np.ndarray: if not OPENCV_CUDA_AVAILABLE or image.size < min_pixels: return cv2.resize(image, size, interpolation=interpolation) try: gpu = cv2.cuda_GpuMat() gpu.upload(image) return cv2.cuda.resize(gpu, size, interpolation=interpolation).download() except cv2.error: return cv2.resize(image, size, interpolation=interpolation) def cuda_cvt_color( image: np.ndarray, code: int, min_pixels: int = CUDA_MIN_PIXELS, ) -> np.ndarray: if not OPENCV_CUDA_AVAILABLE or image.size < min_pixels: return cv2.cvtColor(image, code) try: gpu = cv2.cuda_GpuMat() gpu.upload(image) return cv2.cuda.cvtColor(gpu, code).download() except cv2.error: return cv2.cvtColor(image, code) @dataclass(frozen=True) class Detection: class_id: int class_name: str confidence: float bbox: tuple[int, int, int, int] @dataclass(frozen=True) class CandidateSnapshot: frame_id: int timestamp: float capture_pose: dict[str, object] frame: np.ndarray bbox: tuple[int, int, int, int] label_bbox: tuple[int, int, int, int] label_confidence: float score: float center_score: float size_score: float cut_score: float @dataclass class Track: id: int bbox: tuple[int, int, int, int] confidence: float first_seen_frame: int last_seen_frame: int hits: int = 1 missed: int = 0 state: str = "entering" last_candidate_reason: str = "" pending_remote_response: str = "none" already_snapshotted: bool = False bbox_history: list[tuple[int, int, int, int]] = field(default_factory=list) center_history: list[tuple[float, float]] = field(default_factory=list) area_history: list[float] = field(default_factory=list) candidates: list[CandidateSnapshot] = field(default_factory=list) def __post_init__(self) -> None: self._append_history(self.bbox) def update(self, bbox: tuple[int, int, int, int], confidence: float, frame_id: int) -> None: self.bbox = bbox self.confidence = confidence self.last_seen_frame = frame_id self.hits += 1 self.missed = 0 self._append_history(bbox) def mark_missed(self) -> None: self.missed += 1 if self.missed > 0 and self.state != "snapshotted": self.state = "exiting" def _append_history(self, bbox: tuple[int, int, int, int]) -> None: self.bbox_history.append(bbox) self.center_history.append(bbox_center(bbox)) self.area_history.append(float(bbox_area(bbox))) keep = 20 self.bbox_history = self.bbox_history[-keep:] self.center_history = self.center_history[-keep:] self.area_history = self.area_history[-keep:] def area_trend(self) -> float: if len(self.area_history) < 4: return 0.0 old = self.area_history[-4] new = self.area_history[-1] return (new - old) / max(old, 1.0) @dataclass(frozen=True) class NavigationSnapshot: snapshot_id: int frame_id: int timestamp: float capture_pose: dict[str, object] simulated_position: str track_id: int bbox: tuple[int, int, int, int] label_bbox: tuple[int, int, int, int] score: float debug_frame_path: str ocr_payload_path: str label_payload_path: str movement_vector_px: tuple[float, float] @dataclass(frozen=True) class CapturedFrame: frame_id: int timestamp: float video_time_sec: float frame: np.ndarray pose: dict[str, object] read_ms: float @dataclass(frozen=True) class WmsSnapshotJob: request_id: str snapshot: NavigationSnapshot metadata: dict[str, object] label_image_path: str gaylord_image_path: str | None @dataclass(frozen=True) class WmsResult: request_id: str snapshot_id: int status: str message: str response: dict[str, object] | None = None error: str = "" class UltralyticsDetector: def __init__(self, model_path: str, device: str, half: bool): from ultralytics import YOLO self.model = YOLO(model_path) self.device = device self.half = bool(half) and str(device).strip().lower() != "cpu" names = self.model.names if isinstance(names, dict): self.classes = [str(names[i]) for i in sorted(names)] else: self.classes = [str(name) for name in names] def detect( self, frame: np.ndarray, min_confidence: float, input_size: int, ) -> tuple[list[Detection], float]: t0 = time.perf_counter() results = self.model.predict( source=frame, imgsz=input_size, conf=min_confidence, device=self.device, half=self.half, verbose=False, ) elapsed_ms = (time.perf_counter() - t0) * 1000.0 detections: list[Detection] = [] if not results: return detections, elapsed_ms boxes = results[0].boxes if boxes is None: return detections, elapsed_ms xyxy = boxes.xyxy.cpu().numpy() confs = boxes.conf.cpu().numpy() clss = boxes.cls.cpu().numpy().astype(int) for box, conf, cls_id in zip(xyxy, confs, clss): x1, y1, x2, y2 = [int(round(v)) for v in box] x1, y1, x2, y2 = clip_box(x1, y1, x2, y2, frame.shape[1], frame.shape[0]) if x2 <= x1 or y2 <= y1: continue class_name = self.classes[cls_id] if 0 <= cls_id < len(self.classes) else str(cls_id) detections.append(Detection( class_id=int(cls_id), class_name=class_name, confidence=float(conf), bbox=(x1, y1, x2, y2), )) return detections, elapsed_ms class WmsAsyncClient: def __init__(self, args): self.args = args self.enabled = bool(args.wms_enabled) self.jobs: queue.Queue[WmsSnapshotJob | None] = queue.Queue(maxsize=args.wms_queue_max_size) self.results: queue.Queue[WmsResult] = queue.Queue() self._stop = threading.Event() self._thread: threading.Thread | None = None if self.enabled: self._thread = threading.Thread(target=self._worker, name="flywms-wms-client", daemon=True) self._thread.start() def close(self) -> None: if not self.enabled: return self._stop.set() try: self.jobs.put_nowait(None) except queue.Full: pass if self._thread is not None: self._thread.join(timeout=2.0) def submit(self, snapshot: NavigationSnapshot) -> str | None: if not self.enabled: return None request_id = str(uuid.uuid4()) metadata = { "request_id": request_id, "client_id": self.args.wms_client_id, "snapshot_id": snapshot.snapshot_id, "frame_id": snapshot.frame_id, "timestamp": snapshot.timestamp, "simulated_position": snapshot.simulated_position, "track_id": snapshot.track_id, "gaylord_bbox": list(snapshot.bbox), "label_bbox": list(snapshot.label_bbox), "movement_vector_px": { "dx": snapshot.movement_vector_px[0], "dy": snapshot.movement_vector_px[1], }, } job = WmsSnapshotJob( request_id=request_id, snapshot=snapshot, metadata=metadata, label_image_path=snapshot.label_payload_path, gaylord_image_path=snapshot.debug_frame_path if self.args.wms_send_gaylord_debug else None, ) try: self.jobs.put_nowait(job) except queue.Full: self.results.put(WmsResult( request_id=request_id, snapshot_id=snapshot.snapshot_id, status="ERROR", message="WMS_QUEUE_FULL", error="queue full", )) return request_id log(f"[WMS] job accodato request_id={request_id} snapshot={snapshot.snapshot_id}") return request_id def poll_results(self) -> list[WmsResult]: results: list[WmsResult] = [] while True: try: results.append(self.results.get_nowait()) except queue.Empty: return results def wait_for_result(self, request_id: str | None, timeout_sec: float) -> WmsResult | None: if not request_id: return None deadline = time.perf_counter() + max(0.0, timeout_sec) deferred: list[WmsResult] = [] try: while time.perf_counter() <= deadline: try: result = self.results.get(timeout=0.05) except queue.Empty: continue if result.request_id == request_id: return result deferred.append(result) return None finally: for result in deferred: self.results.put(result) def _worker(self) -> None: while not self._stop.is_set(): try: job = self.jobs.get(timeout=0.2) except queue.Empty: continue if job is None: break result = self._send_job(job) self.results.put(result) def _send_job(self, job: WmsSnapshotJob) -> WmsResult: try: body, content_type = build_wms_multipart(job) req = request.Request( self.args.wms_server_url, data=body, headers={"Content-Type": content_type}, method="POST", ) with request.urlopen(req, timeout=self.args.wms_timeout_sec) as response: payload = json.loads(response.read().decode("utf-8")) status = str(payload.get("status", "ERROR")) message = str(payload.get("message", "")) log(f"[WMS] risposta request_id={job.request_id} status={status} message={message}") return WmsResult(job.request_id, job.snapshot.snapshot_id, status, message, payload) except (OSError, error.URLError, TimeoutError, json.JSONDecodeError) as exc: log(f"[WMS] errore request_id={job.request_id}: {exc}") return WmsResult( request_id=job.request_id, snapshot_id=job.snapshot.snapshot_id, status="ERROR", message="WMS_SEND_ERROR", error=str(exc), ) class ObserverPublisher: def __init__(self, args): self.enabled = bool(getattr(args, "observer_enabled", False)) self.host = str(getattr(args, "observer_host", "127.0.0.1")) self.port = int(getattr(args, "observer_port", 8765)) self._queue: queue.Queue[dict[str, object] | None] = queue.Queue(maxsize=32) self._stop = threading.Event() self._thread: threading.Thread | None = None if self.enabled: self._thread = threading.Thread(target=self._worker, name="flywms-observer-publisher", daemon=True) self._thread.start() def close(self) -> None: if not self.enabled: return self._stop.set() self._enqueue(None) if self._thread is not None: self._thread.join(timeout=2.0) def publish_telemetry(self, payload: dict[str, object]) -> None: if not self.enabled: return self._enqueue({ "type": "telemetry", **payload, }) def publish_preview( self, stream: str, frame: np.ndarray, frame_id: int, timestamp: float, jpeg_quality: int, ) -> None: if not self.enabled or frame is None: return ok, encoded = cv2.imencode( ".jpg", frame, [int(cv2.IMWRITE_JPEG_QUALITY), int(max(20, min(100, jpeg_quality)))], ) if not ok: return self._enqueue({ "type": "preview", "stream": stream, "frame_id": int(frame_id), "timestamp": float(timestamp), "width": int(frame.shape[1]), "height": int(frame.shape[0]), "encoding": "jpeg-base64", "image_b64": base64.b64encode(encoded.tobytes()).decode("ascii"), }) def _enqueue(self, payload: dict[str, object] | None) -> None: try: self._queue.put_nowait(payload) return except queue.Full: try: self._queue.get_nowait() except queue.Empty: pass try: self._queue.put_nowait(payload) except queue.Full: pass def _worker(self) -> None: server: socket.socket | None = None conn: socket.socket | None = None try: server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) server.bind((self.host, self.port)) server.listen(1) server.settimeout(0.5) log(f"[OBS] publisher in ascolto su {self.host}:{self.port}") while not self._stop.is_set(): if conn is None: try: conn, addr = server.accept() conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) log(f"[OBS] observer connesso da {addr[0]}:{addr[1]}") except socket.timeout: continue except OSError: break try: payload = self._queue.get(timeout=0.2) except queue.Empty: continue if payload is None: break try: raw = (json.dumps(payload, ensure_ascii=True) + "\n").encode("utf-8") conn.sendall(raw) except OSError: try: conn.close() except OSError: pass conn = None finally: if conn is not None: try: conn.close() except OSError: pass if server is not None: try: server.close() except OSError: pass class LightweightTracker: """Greedy bbox tracker: enough to explain and test navigation decisions.""" def __init__( self, max_missed: int, min_match_score: float, max_center_distance_ratio: float, ): self.max_missed = max_missed self.min_match_score = min_match_score self.max_center_distance_ratio = max_center_distance_ratio self._next_id = 1 self.tracks: dict[int, Track] = {} def update( self, detections: list[Detection], frame_id: int, frame_width: int, ) -> list[Track]: unmatched_tracks = set(self.tracks.keys()) unmatched_detections = set(range(len(detections))) pairs: list[tuple[float, int, int]] = [] max_center_distance = max(1.0, frame_width * self.max_center_distance_ratio) for track_id, track in self.tracks.items(): for det_idx, det in enumerate(detections): score = association_score(track.bbox, det.bbox, max_center_distance) if score >= self.min_match_score: pairs.append((score, track_id, det_idx)) pairs.sort(reverse=True, key=lambda item: item[0]) for _, track_id, det_idx in pairs: if track_id not in unmatched_tracks or det_idx not in unmatched_detections: continue det = detections[det_idx] self.tracks[track_id].update(det.bbox, det.confidence, frame_id) unmatched_tracks.remove(track_id) unmatched_detections.remove(det_idx) for track_id in list(unmatched_tracks): self.tracks[track_id].mark_missed() if self.tracks[track_id].missed > self.max_missed: del self.tracks[track_id] for det_idx in unmatched_detections: det = detections[det_idx] track_id = self._next_id self._next_id += 1 self.tracks[track_id] = Track( id=track_id, bbox=det.bbox, confidence=det.confidence, first_seen_frame=frame_id, last_seen_frame=frame_id, ) return list(self.tracks.values()) class NavigationController: def __init__(self, args): self.args = args self.output_dir = Path(args.snapshot_output_dir) self.output_dir.mkdir(parents=True, exist_ok=True) self.metadata_path = self.output_dir / "snapshots.jsonl" self.snapshot_counter = 0 self.position_counter = 0 self.last_command_text = "" self.last_command_lines: list[str] = [] self.last_snapshot_frame: np.ndarray | None = None self.last_ocr_payload_frame: np.ndarray | None = None self.last_label_payload_frame: np.ndarray | None = None self.last_remote_result_text = "" self.motion_text = "MOTO: n/d" self.label_movement_arrow: tuple[tuple[int, int], tuple[int, int]] | None = None def process_track( self, track: Track, captured: CapturedFrame, labels: list[Detection] | None = None, ) -> NavigationSnapshot | None: labels = labels or [] frame = captured.frame frame_id = captured.frame_id timestamp = captured.timestamp frame_h, frame_w = frame.shape[:2] eligible, score_parts = self._is_snapshot_candidate(track, frame_w, frame_h) self._update_track_state(track, eligible, frame_w) if track.already_snapshotted: return None if eligible: label = find_label_inside_bbox(track.bbox, labels) if label is None: track.last_candidate_reason = "label_missing" self.last_command_text = f"ETICHETTA_NON_TROVATA track={track.id}" self.last_command_lines = [ self.last_command_text, "MICRO_RICERCA_ETICHETTA", "ATTENDI_FRAME_SUCCESSIVO", ] log(f"[NAV] ETICHETTA_NON_TROVATA track={track.id}: attendo frame successivo") return None candidate = CandidateSnapshot( frame_id=frame_id, timestamp=timestamp, capture_pose=dict(captured.pose), frame=frame.copy(), bbox=track.bbox, label_bbox=label.bbox, label_confidence=label.confidence, score=score_parts["score"], center_score=score_parts["center_score"], size_score=score_parts["size_score"], cut_score=score_parts["cut_score"], ) track.candidates.append(candidate) track.candidates = track.candidates[-self.args.snapshot_window_frames:] if len(track.candidates) >= self.args.snapshot_window_frames: return self._finalize_snapshot(track) elif track.candidates: return self._finalize_snapshot(track) return None def _is_snapshot_candidate( self, track: Track, frame_w: int, frame_h: int, ) -> tuple[bool, dict[str, float]]: x1, y1, x2, y2 = track.bbox cx, cy = bbox_center(track.bbox) center_x = frame_w * 0.5 center_tolerance = max(1.0, frame_w * self.args.center_tolerance_ratio) snapshot_tolerance = max(1.0, frame_w * self.args.snapshot_line_tolerance_ratio) center_delta = abs(cx - center_x) center_score = max(0.0, 1.0 - center_delta / center_tolerance) area_ratio = bbox_area(track.bbox) / float(frame_w * frame_h) size_score = min(1.0, area_ratio / max(self.args.min_gaylord_area_ratio * 4.0, 0.001)) if self.args.edge_margin_ratio <= 0: cut = False else: edge_margin_x = frame_w * self.args.edge_margin_ratio edge_margin_y = frame_h * self.args.edge_margin_ratio cut = ( x1 <= edge_margin_x or y1 <= edge_margin_y or x2 >= frame_w - edge_margin_x or y2 >= frame_h - edge_margin_y ) cut_score = 0.0 if cut else 1.0 score = 0.50 * center_score + 0.30 * size_score + 0.20 * cut_score in_center_band = center_delta <= center_tolerance on_snapshot_line = center_delta <= snapshot_tolerance in_y_band = ( frame_h * self.args.usable_y_min_ratio <= cy <= frame_h * self.args.usable_y_max_ratio ) enough_hits = track.hits >= self.args.min_track_hits large_enough = area_ratio >= self.args.min_gaylord_area_ratio trend_ok = track.area_trend() >= self.args.min_area_trend eligible = ( enough_hits and on_snapshot_line and in_y_band and large_enough and not cut and trend_ok and track.missed == 0 ) failed: list[str] = [] if not enough_hits: failed.append(f"hits<{self.args.min_track_hits}") if not in_center_band: failed.append(f"outside_band={center_delta:.0f}>{center_tolerance:.0f}") elif not on_snapshot_line: failed.append(f"wait_line={center_delta:.0f}>{snapshot_tolerance:.0f}") if not in_y_band: failed.append("y_band") if not large_enough: failed.append(f"area={area_ratio:.3f}<{self.args.min_gaylord_area_ratio:.3f}") if cut: failed.append("edge_cut") if not trend_ok: failed.append(f"trend={track.area_trend():+.2f}<{self.args.min_area_trend:+.2f}") if track.missed != 0: failed.append(f"missed={track.missed}") track.last_candidate_reason = "ok" if eligible else ",".join(failed) return eligible, { "score": score, "center_score": center_score, "size_score": size_score, "cut_score": cut_score, } def _update_track_state(self, track: Track, eligible: bool, frame_w: int) -> None: if track.already_snapshotted: track.state = "snapshotted" return if track.missed > 0: track.state = "exiting" return cx, _ = bbox_center(track.bbox) center_delta = abs(cx - frame_w * 0.5) snapshot_tolerance = frame_w * self.args.snapshot_line_tolerance_ratio if eligible: track.state = "centered" elif track.hits < self.args.min_track_hits: track.state = "entering" elif center_delta <= snapshot_tolerance: track.state = "centered" elif center_delta <= frame_w * self.args.center_tolerance_ratio: track.state = "candidate" else: track.state = "entering" def _finalize_snapshot(self, track: Track) -> NavigationSnapshot | None: if not track.candidates: return None best = max(track.candidates, key=lambda item: item.score) track.candidates.clear() track.already_snapshotted = True track.state = "snapshotted" self.snapshot_counter += 1 self.position_counter += 1 simulated_position = f"gaylord {self.position_counter}" debug_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_frame.jpg" payload_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_ocr_payload.jpg" label_payload_name = f"snapshot_{self.snapshot_counter:04d}_track_{track.id:03d}_label_payload.jpg" debug_path = self.output_dir / debug_name payload_path = self.output_dir / payload_name label_payload_path = self.output_dir / label_payload_name cv2.imwrite(str(debug_path), best.frame) ocr_payload = crop_with_padding( best.frame, best.bbox, self.args.ocr_payload_pad_ratio, ) cv2.imwrite(str(payload_path), ocr_payload) label_payload = crop_with_padding( best.frame, best.label_bbox, self.args.label_payload_pad_ratio, ) cv2.imwrite(str(label_payload_path), label_payload) self.last_snapshot_frame = best.frame.copy() self.last_ocr_payload_frame = ocr_payload.copy() self.last_label_payload_frame = label_payload.copy() gaylord_center = bbox_center(best.bbox) label_center = bbox_center(best.label_bbox) movement_vector = ( label_center[0] - gaylord_center[0], label_center[1] - gaylord_center[1], ) self.label_movement_arrow = ( (int(round(gaylord_center[0])), int(round(gaylord_center[1]))), (int(round(label_center[0])), int(round(label_center[1]))), ) snapshot = NavigationSnapshot( snapshot_id=self.snapshot_counter, frame_id=best.frame_id, timestamp=best.timestamp, capture_pose=dict(best.capture_pose), simulated_position=simulated_position, track_id=track.id, bbox=best.bbox, label_bbox=best.label_bbox, score=best.score, debug_frame_path=str(debug_path), ocr_payload_path=str(payload_path), label_payload_path=str(label_payload_path), movement_vector_px=movement_vector, ) self._write_metadata(snapshot) self._print_commands(snapshot) return snapshot def simulate_remote_response(self, snapshot: NavigationSnapshot) -> str: mode = self.args.remote_ack_mode if mode == "always-ack": result = "ACK" elif mode == "always-nack": result = "NACK" else: result = "ACK" if snapshot.snapshot_id % 2 == 1 else "NACK" if result == "ACK": self.last_remote_result_text = "ACK_RICEVUTO: codice valido su WMS" resume_command = f"RIPARTI_{self.args.scan_direction.upper()}" self.last_command_lines.extend([ self.last_remote_result_text, resume_command, ]) log("[REMOTE] ACK_RICEVUTO codice valido su WMS") log(f"[CMD] {resume_command}") else: self.last_remote_result_text = "NACK_RICEVUTO: riprovare foto" self.last_command_lines.extend([ self.last_remote_result_text, "MICRO_MOVE_CORRETTIVO", "SCATTA_FOTO_RETRY", ]) log("[REMOTE] NACK_RICEVUTO codice assente/non valido") log("[CMD] MICRO_MOVE_CORRETTIVO") log("[CMD] SCATTA_FOTO_RETRY") return result def apply_wms_result(self, result: WmsResult | None, snapshot: NavigationSnapshot) -> str: if result is None: self.last_remote_result_text = "WMS_TIMEOUT: nessuna risposta entro il timeout" self.last_command_lines.extend([ self.last_remote_result_text, "MICRO_MOVE_CORRETTIVO", "SCATTA_FOTO_RETRY", ]) log("[WMS] TIMEOUT nessuna risposta entro il timeout") return "TIMEOUT" status = result.status.upper() if status == "ACK": message = result.message or "codice valido su WMS" ocr_text = "" if result.response: ocr_text = str(result.response.get("ocr_text") or result.response.get("fake_ocr_text") or "") self.last_remote_result_text = f"WMS_ACK: {message}" resume_command = f"RIPARTI_{self.args.scan_direction.upper()}" lines = [self.last_remote_result_text] if ocr_text: lines.append(f"OCR_CODICE {ocr_text}") lines.append(resume_command) self.last_command_lines.extend(lines) log(f"[WMS] ACK request_id={result.request_id} snapshot={result.snapshot_id} {message}") log(f"[CMD] {resume_command}") return "ACK" if status == "NACK": message = result.message or "codice non riconosciuto" self.last_remote_result_text = f"WMS_NACK: {message}" self.last_command_lines.extend([ self.last_remote_result_text, "MICRO_MOVE_CORRETTIVO", "SCATTA_FOTO_RETRY", ]) log(f"[WMS] NACK request_id={result.request_id} snapshot={result.snapshot_id} {message}") return "NACK" message = result.error or result.message or "errore WMS" self.last_remote_result_text = f"WMS_ERROR: {message}" self.last_command_lines.extend([ self.last_remote_result_text, "MICRO_MOVE_CORRETTIVO", "SCATTA_FOTO_RETRY", ]) log(f"[WMS] ERROR request_id={result.request_id} snapshot={result.snapshot_id} {message}") return "ERROR" def set_motion_text(self, text: str) -> None: self.motion_text = text def _write_metadata(self, snapshot: NavigationSnapshot) -> None: record = { "snapshot_id": snapshot.snapshot_id, "frame_id": snapshot.frame_id, "timestamp": snapshot.timestamp, "simulated_position": snapshot.simulated_position, "drone_pose_simulated": dict(snapshot.capture_pose), "track_id": snapshot.track_id, "gaylord_bbox": list(snapshot.bbox), "label_bbox": list(snapshot.label_bbox), "score": snapshot.score, "movement_vector_px": { "dx": snapshot.movement_vector_px[0], "dy": snapshot.movement_vector_px[1], }, "debug_frame_path": snapshot.debug_frame_path, "ocr_payload_path": snapshot.ocr_payload_path, "label_payload_path": snapshot.label_payload_path, } with self.metadata_path.open("at", encoding="utf-8") as f: f.write(json.dumps(record, ensure_ascii=True) + "\n") def _print_commands(self, snapshot: NavigationSnapshot) -> None: self.last_command_text = ( f"SNAPSHOT {snapshot.snapshot_id:04d} " f"track={snapshot.track_id} frame={snapshot.frame_id} " f"pos={snapshot.simulated_position} score={snapshot.score:.2f}" ) dx, dy = snapshot.movement_vector_px self.last_command_lines = [ self.last_command_text, "STOP", f"ASSOCIA_POSIZIONE {snapshot.simulated_position}", f"ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}", f"CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s", f"ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s", f"SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}", f"RITORNA_CENTRO_GAYLORD durata={self.args.label_return_sec:.1f}s", f"SCATTA_FOTO_GAYLORD {Path(snapshot.debug_frame_path).name}", f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}", f"ATTENDI_ACK timeout={self.args.remote_ack_timeout_sec:.1f}s", ] log(f"[NAV] {self.last_command_text}") log("[CMD] STOP") log(f"[CMD] ASSOCIA_POSIZIONE {snapshot.simulated_position}") log(f"[CMD] ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}") log(f"[CMD] CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s") log(f"[CMD] ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s") log(f"[CMD] SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}") log(f"[CMD] RITORNA_CENTRO_GAYLORD durata={self.args.label_return_sec:.1f}s") log(f"[CMD] SCATTA_FOTO_GAYLORD {Path(snapshot.debug_frame_path).name}") log(f"[CMD] INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}") log(f"[CMD] ATTENDI_ACK timeout={self.args.remote_ack_timeout_sec:.1f}s") def set_label_sequence_phase(self, snapshot: NavigationSnapshot, phase: str) -> None: dx, dy = snapshot.movement_vector_px base = [ self.last_command_text, "STOP", f"ASSOCIA_POSIZIONE {snapshot.simulated_position}", f"ASSOCIA_ETICHETTA {snapshot.simulated_position} - etichetta {snapshot.snapshot_id}", ] if phase == "move": detail = [ f"CENTRA_ETICHETTA dx={dx:+.0f}px dy={dy:+.0f}px durata={self.args.label_move_sec:.1f}s", "FRECCIA_MOVIMENTO_GAYLORD_ETICHETTA", ] elif phase == "stabilize": detail = [ "DRONE_CENTRATO_SU_ETICHETTA", f"ATTENDI_STABILIZZAZIONE {self.args.label_stabilization_sec:.1f}s", ] elif phase == "capture": detail = [ f"SCATTA_FOTO_ETICHETTA {Path(snapshot.label_payload_path).name}", f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}", ] elif phase == "wait_wms": detail = [ f"INVIA_ROI_REMOTA {Path(snapshot.label_payload_path).name}", f"ATTENDI_WMS timeout={self.args.remote_ack_timeout_sec:.1f}s", ] elif phase == "return": rdx = -dx rdy = -dy detail = [ f"RITORNA_CENTRO_GAYLORD dx={rdx:+.0f}px dy={rdy:+.0f}px durata={self.args.label_return_sec:.1f}s", "FRECCIA_RITORNO_ETICHETTA_GAYLORD", ] else: detail = [] self.last_command_lines = base + detail def parse_args(): pre = argparse.ArgumentParser(add_help=False) pre.add_argument("--config", default=DEFAULT_CONFIG_PATH, help="File configurazione INI") pre_args, _ = pre.parse_known_args() defaults = load_navigation_config(pre_args.config) ap = argparse.ArgumentParser(parents=[pre]) ap.add_argument("-v", "--video", default=defaults["video"], help="Percorso video. Se omesso usa webcam 0") ap.add_argument( "--weights", default=defaults["weights"], help="Modello Ultralytics .pt", ) ap.add_argument("--ultralytics-device", default=defaults["ultralytics_device"], help="Device Ultralytics: cpu oppure 0") ap.add_argument("--yolo-half", action=argparse.BooleanOptionalAction, default=defaults["yolo_half"], help="Usa FP16/half precision per YOLO quando il device e' GPU") ap.add_argument("--input-size", type=int, default=defaults["input_size"], help="Dimensione input YOLO") ap.add_argument("--min-confidence", type=float, default=defaults["min_confidence"], help="Confidenza minima") ap.add_argument("--target-class", default=defaults["target_class"], help="Classe da tracciare") ap.add_argument("--label-class", default=defaults["label_class"], help="Classe etichetta da associare al gaylord") ap.add_argument("--max-track-missed", type=int, default=defaults["max_track_missed"], help="Frame persi prima di rimuovere una track") ap.add_argument("--min-match-score", type=float, default=defaults["min_match_score"], help="Soglia associazione detection-track") ap.add_argument("--max-center-distance-ratio", type=float, default=defaults["max_center_distance_ratio"], help="Distanza max centri per matching") ap.add_argument("--center-tolerance-ratio", type=float, default=defaults["center_tolerance_ratio"], help="Mezza ampiezza zona centrale") ap.add_argument("--snapshot-line-tolerance-ratio", type=float, default=defaults["snapshot_line_tolerance_ratio"], help="Tolleranza stretta dalla linea centrale per scattare") ap.add_argument("--usable-y-min-ratio", type=float, default=defaults["usable_y_min_ratio"], help="Limite alto fascia utile Y") ap.add_argument("--usable-y-max-ratio", type=float, default=defaults["usable_y_max_ratio"], help="Limite basso fascia utile Y") ap.add_argument("--min-track-hits", type=int, default=defaults["min_track_hits"], help="Detection consecutive minime") ap.add_argument("--min-gaylord-area-ratio", type=float, default=defaults["min_gaylord_area_ratio"], help="Area bbox minima sul frame") ap.add_argument("--edge-margin-ratio", type=float, default=defaults["edge_margin_ratio"], help="Margine per considerare bbox tagliato") ap.add_argument("--ocr-payload-pad-ratio", type=float, default=defaults["ocr_payload_pad_ratio"], help="Padding intorno al bbox centrale inviato all'OCR remoto") ap.add_argument("--label-payload-pad-ratio", type=float, default=defaults["label_payload_pad_ratio"], help="Padding intorno al bbox etichetta inviato all'OCR remoto") ap.add_argument("--min-area-trend", type=float, default=defaults["min_area_trend"], help="Trend area minimo ammesso") ap.add_argument("--snapshot-window-frames", type=int, default=defaults["snapshot_window_frames"], help="Candidati da valutare prima dello snapshot") ap.add_argument("--snapshot-output-dir", default=defaults["snapshot_output_dir"], help="Directory snapshot e JSONL") ap.add_argument("--remote-ack-timeout-sec", type=float, default=defaults["remote_ack_timeout_sec"], help="Tempo simulato di attesa OCR remoto/WMS") ap.add_argument("--remote-ack-mode", choices=["always-ack", "always-nack", "alternate"], default=defaults["remote_ack_mode"], help="Risposta remota simulata") ap.add_argument("--wms-enabled", action="store_true", default=defaults["wms_enabled"], help="Invia realmente snapshot al WMS demo") ap.add_argument("--wms-server-url", default=defaults["wms_server_url"], help="Endpoint HTTP WMS") ap.add_argument("--wms-client-id", default=defaults["wms_client_id"], help="Identificativo client WMS") ap.add_argument("--wms-timeout-sec", type=float, default=defaults["wms_timeout_sec"], help="Timeout chiamata WMS") ap.add_argument("--wms-queue-max-size", type=int, default=defaults["wms_queue_max_size"], help="Massimo job WMS in coda") ap.add_argument("--wms-send-gaylord-debug", action="store_true", default=defaults["wms_send_gaylord_debug"], help="Invia anche immagine/debug gaylord al server WMS") ap.add_argument("--scan-direction", choices=["destra", "sinistra"], default=defaults["scan_direction"], help="Direzione simulata di ripartenza dopo ACK") ap.add_argument("--label-move-sec", type=float, default=defaults["label_move_sec"], help="Durata simulata movimento verso etichetta") ap.add_argument("--label-stabilization-sec", type=float, default=defaults["label_stabilization_sec"], help="Attesa simulata stabilizzazione su etichetta") ap.add_argument("--label-return-sec", type=float, default=defaults["label_return_sec"], help="Durata simulata ritorno al centro gaylord") ap.add_argument("--preview-width", type=int, default=defaults["preview_width"], help="Larghezza preview") ap.add_argument("--benchmark-mode", action=argparse.BooleanOptionalAction, default=defaults["benchmark_mode"], help="Profilo benchmark: niente finestre, preview a 30 fps e log tempi dedicato") ap.add_argument("--benchmark-preview-fps", type=float, default=defaults["benchmark_preview_fps"], help="FPS target preview/cattura usati dal profilo benchmark") ap.add_argument("--realtime-playback", action="store_true", default=defaults["realtime_playback"], help="Rispetta FPS video") ap.add_argument("--preview-fps", type=float, default=defaults["preview_fps"], help="FPS massimo per lettura/preview realtime. 0 = FPS sorgente") ap.add_argument("--yolo-fps", type=float, default=defaults["yolo_fps"], help="FPS massimo per inferenza YOLO. 0 = ogni frame di preview") ap.add_argument("--adaptive-yolo-enabled", action=argparse.BooleanOptionalAction, default=defaults["adaptive_yolo_enabled"], help="Abilita scheduling adattivo di YOLO in base allo stato delle track") ap.add_argument("--idle-yolo-fps", type=float, default=defaults["idle_yolo_fps"], help="FPS YOLO quando non ci sono track attive") ap.add_argument("--tracking-yolo-fps", type=float, default=defaults["tracking_yolo_fps"], help="FPS YOLO quando ci sono track attive ma non ancora critiche") ap.add_argument("--critical-yolo-fps", type=float, default=defaults["critical_yolo_fps"], help="FPS YOLO quando una track e' candidate/centered") ap.add_argument("--max-frames", type=int, default=defaults["max_frames"], help="Numero massimo frame; 0 = tutto") ap.add_argument("--stats-interval", type=float, default=defaults["stats_interval"], help="Intervallo log prestazioni") ap.add_argument("--perf-log-path", default=defaults["perf_log_path"], help="File log tempi dettagliati") ap.add_argument("--perf-log-flush-interval-sec", type=float, default=defaults["perf_log_flush_interval_sec"], help="Intervallo flush file log tempi") ap.add_argument("--perf-log-flush-lines", type=int, default=defaults["perf_log_flush_lines"], help="Numero righe bufferizzate prima del flush del log tempi") ap.add_argument("--motion-report-interval", type=int, default=defaults["motion_report_interval"], help="Ogni quanti frame aggiornare la direzione moto stimata") ap.add_argument("--motion-min-pixels", type=float, default=defaults["motion_min_pixels"], help="Spostamento medio minimo per dichiarare una direzione") ap.add_argument("--debug-tracks", action="store_true", default=defaults["debug_tracks"], help="Logga stato e criteri delle track") ap.add_argument("--flash-alpha", type=float, default=defaults["flash_alpha"], help="Intensita' flash 0..1 al momento dello scatto") ap.add_argument("--no-display", action="store_true", default=defaults["no_display"], help="Disabilita finestra video") ap.add_argument("--observer-enabled", action=argparse.BooleanOptionalAction, default=defaults["observer_enabled"], help="Abilita observer esterno su socket locale; la UI locale viene disattivata") ap.add_argument("--observer-host", default=defaults["observer_host"], help="Host observer, tipicamente 127.0.0.1") ap.add_argument("--observer-port", type=int, default=defaults["observer_port"], help="Porta TCP observer") ap.add_argument("--observer-preview-fps", type=float, default=defaults["observer_preview_fps"], help="FPS massimi delle preview inviate all'observer") ap.add_argument("--observer-preview-width", type=int, default=defaults["observer_preview_width"], help="Larghezza massima delle preview inviate all'observer") ap.add_argument("--observer-jpeg-quality", type=int, default=defaults["observer_jpeg_quality"], help="Qualita' JPEG 20..100 per le preview inviate all'observer") ap.add_argument("--observer-telemetry-fps", type=float, default=defaults["observer_telemetry_fps"], help="Frequenza telemetria observer in Hz") ap.add_argument("--window-layout-enabled", action="store_true", default=defaults["window_layout_enabled"], help="Posiziona e ridimensiona le finestre OpenCV") ap.add_argument("--navigate-window", default=defaults["navigate_window"], help="Layout finestra navigate: x,y,w,h") ap.add_argument("--commands-window", default=defaults["commands_window"], help="Layout finestra comandi: x,y,w,h") ap.add_argument("--snapshot-window", default=defaults["snapshot_window"], help="Layout finestra snapshot: x,y,w,h") ap.add_argument("--label-window", default=defaults["label_window"], help="Layout finestra etichetta: x,y,w,h") return ap.parse_args() def load_navigation_config(path_str: str) -> dict[str, object]: defaults: dict[str, object] = { "video": "testhd.mp4", "weights": r"C:\devel\flywms\runs\flywms_yolo11n_quick20\weights\best.pt", "ultralytics_device": "cpu", "yolo_half": True, "input_size": 640, "min_confidence": 0.25, "target_class": "gaylord", "label_class": "etichetta", "max_track_missed": 8, "min_match_score": 0.25, "max_center_distance_ratio": 0.18, "center_tolerance_ratio": 0.18, "snapshot_line_tolerance_ratio": 0.035, "usable_y_min_ratio": 0.15, "usable_y_max_ratio": 0.85, "min_track_hits": 3, "min_gaylord_area_ratio": 0.02, "edge_margin_ratio": 0.0, "ocr_payload_pad_ratio": 0.03, "label_payload_pad_ratio": 0.20, "min_area_trend": -0.35, "snapshot_window_frames": 1, "snapshot_output_dir": "navigate_snapshots", "remote_ack_timeout_sec": 3.0, "remote_ack_mode": "always-ack", "wms_enabled": False, "wms_server_url": "http://127.0.0.1:8088/api/v1/navigation-snapshot", "wms_client_id": "flywms-demo-01", "wms_timeout_sec": 2.0, "wms_queue_max_size": 8, "wms_send_gaylord_debug": True, "scan_direction": "destra", "label_move_sec": 3.0, "label_stabilization_sec": 2.0, "label_return_sec": 3.0, "preview_width": 1280, "benchmark_mode": False, "benchmark_preview_fps": 30.0, "realtime_playback": True, "preview_fps": 24.0, "yolo_fps": 15.0, "adaptive_yolo_enabled": False, "idle_yolo_fps": 8.0, "tracking_yolo_fps": 12.0, "critical_yolo_fps": 15.0, "max_frames": 0, "stats_interval": 2.0, "perf_log_path": "tempistiche.txt", "perf_log_flush_interval_sec": 2.0, "perf_log_flush_lines": 120, "motion_report_interval": 5, "motion_min_pixels": 1.5, "debug_tracks": True, "flash_alpha": 0.70, "no_display": False, "observer_enabled": False, "observer_host": "127.0.0.1", "observer_port": 8765, "observer_preview_fps": 4.0, "observer_preview_width": 960, "observer_jpeg_quality": 75, "observer_telemetry_fps": 8.0, "window_layout_enabled": True, "navigate_window": "20,40,1100,620", "commands_window": "1140,40,760,520", "snapshot_window": "1140,590,520,360", "label_window": "1140,980,520,260", } path = Path(path_str) if not path.exists(): return defaults parser = configparser.ConfigParser() parser.read(path, encoding="utf-8") section = parser["navigation"] if parser.has_section("navigation") else {} types = {key: type(value) for key, value in defaults.items()} for key, default_value in defaults.items(): if key not in section: continue if types[key] is bool: defaults[key] = parser.getboolean("navigation", key, fallback=bool(default_value)) elif types[key] is int: defaults[key] = parser.getint("navigation", key, fallback=int(default_value)) elif types[key] is float: defaults[key] = parser.getfloat("navigation", key, fallback=float(default_value)) else: value = section.get(key, str(default_value)).strip() defaults[key] = None if value.lower() in ("", "none", "null") else value return defaults def log(msg: str) -> None: print(f"[{time.strftime('%H:%M:%S')}] {msg}", flush=True) def format_fps_value(value: float | int | None) -> str: if value is None: return "n/d" try: numeric = float(value) except (TypeError, ValueError): return "n/d" if numeric <= 0: return "n/d" return f"{numeric:.1f}" def format_seconds(value: float | None) -> str: if value is None: return "n/d" try: numeric = float(value) except (TypeError, ValueError): return "n/d" if numeric < 0: numeric = 0.0 minutes = int(numeric // 60) seconds = numeric - (minutes * 60) return f"{minutes:02d}:{seconds:05.2f}" def choose_adaptive_yolo_fps(args, tracks: list[Track]) -> tuple[float, str]: if not getattr(args, "adaptive_yolo_enabled", False): return float(args.yolo_fps or 0.0), "fixed" active_tracks = [t for t in tracks if t.missed == 0 and not t.already_snapshotted] if not active_tracks: return float(args.idle_yolo_fps or 0.0), "idle" critical_states = {"candidate", "centered"} if any(t.state in critical_states for t in active_tracks): return float(args.critical_yolo_fps or 0.0), "critical" return float(args.tracking_yolo_fps or 0.0), "tracking" class PerfLogWriter: def __init__(self, path_str: str, flush_interval_sec: float, flush_lines: int): self.path = Path(path_str) self.path.parent.mkdir(parents=True, exist_ok=True) self.file = self.path.open("a", encoding="utf-8") self.flush_interval_sec = max(0.2, float(flush_interval_sec)) self.flush_lines = max(1, int(flush_lines)) self.buffered_lines = 0 self.last_flush = time.perf_counter() def write(self, line: str) -> None: self.file.write(line.rstrip("\n") + "\n") self.buffered_lines += 1 self.maybe_flush() def maybe_flush(self) -> None: now = time.perf_counter() if self.buffered_lines >= self.flush_lines or (now - self.last_flush) >= self.flush_interval_sec: self.file.flush() self.buffered_lines = 0 self.last_flush = now def close(self) -> None: try: self.file.flush() finally: self.file.close() def safe_progress_ratio(frame_id: int, total_frames: int) -> float: if total_frames <= 0: return 0.0 return min(max((frame_id - 1) / max(total_frames - 1, 1), 0.0), 1.0) def sample_demo_pose( args, frame_id: int, video_time_sec: float, total_frames: int, ) -> dict[str, object]: return { "mode": "video_demo_capture", "frame_id": int(frame_id), "video_time_sec": float(video_time_sec), "scan_direction": str(args.scan_direction), "route_progress_ratio": float(safe_progress_ratio(frame_id, total_frames)), } class CaptureWorker: def __init__( self, cap: cv2.VideoCapture, args, video_fps: float, frame_delay: float, total_frames: int, ) -> None: self.cap = cap self.args = args self.video_fps = float(video_fps or 0.0) self.frame_delay = float(frame_delay or 0.0) self.total_frames = int(total_frames) self.queue: queue.Queue[CapturedFrame | None] = queue.Queue(maxsize=1) self.stop_event = threading.Event() self.thread = threading.Thread(target=self._run, name="capture-worker", daemon=True) self.last_emit = time.perf_counter() def start(self) -> None: self.thread.start() def close(self) -> None: self.stop_event.set() self.thread.join(timeout=2.0) def get(self, timeout: float = 1.0) -> CapturedFrame | None: deadline = time.perf_counter() + max(timeout, 0.1) while True: try: return self.queue.get(timeout=0.1) except queue.Empty: if self.stop_event.is_set() and self.queue.empty(): return None if time.perf_counter() >= deadline: raise def _offer(self, packet: CapturedFrame | None) -> None: while not self.stop_event.is_set(): try: self.queue.put_nowait(packet) return except queue.Full: try: self.queue.get_nowait() except queue.Empty: return def _sleep_for_realtime(self) -> None: if self.frame_delay <= 0: return now = time.perf_counter() sleep_for = self.frame_delay - (now - self.last_emit) if sleep_for > 0: time.sleep(sleep_for) self.last_emit = time.perf_counter() def _run(self) -> None: frame_id = 0 try: while not self.stop_event.is_set(): self._sleep_for_realtime() read_t0 = time.perf_counter() ok, frame = self.cap.read() read_ms = (time.perf_counter() - read_t0) * 1000.0 if not ok: self._offer(None) return frame_id += 1 timestamp = time.perf_counter() pos_msec = float(self.cap.get(cv2.CAP_PROP_POS_MSEC) or 0.0) if pos_msec > 0: video_time_sec = pos_msec / 1000.0 elif self.video_fps > 0: video_time_sec = max(frame_id - 1, 0) / self.video_fps else: video_time_sec = 0.0 pose = sample_demo_pose(self.args, frame_id, video_time_sec, self.total_frames) self._offer(CapturedFrame( frame_id=frame_id, timestamp=timestamp, video_time_sec=video_time_sec, frame=frame, pose=pose, read_ms=read_ms, )) finally: self.stop_event.set() def require_file(path_str: str, description: str) -> Path: path = Path(path_str) if not path.exists(): log(f"ERRORE: {description} non trovato: {path}") sys.exit(1) return path def open_capture(video_arg: str | None): if video_arg is None: cap = cv2.VideoCapture(0, cv2.CAP_DSHOW) if not cap.isOpened(): cap = cv2.VideoCapture(0) return cap, "camera:0" if str(video_arg).isdigit(): idx = int(video_arg) cap = cv2.VideoCapture(idx, cv2.CAP_DSHOW) if not cap.isOpened(): cap = cv2.VideoCapture(idx) return cap, f"camera:{idx}" return cv2.VideoCapture(video_arg), str(video_arg) def build_wms_multipart(job: WmsSnapshotJob) -> tuple[bytes, str]: boundary = f"flywms-{uuid.uuid4().hex}" chunks: list[bytes] = [] def add_field(name: str, value: str, content_type: str = "text/plain") -> None: chunks.extend([ f"--{boundary}\r\n".encode("ascii"), f'Content-Disposition: form-data; name="{name}"\r\n'.encode("ascii"), f"Content-Type: {content_type}\r\n\r\n".encode("ascii"), value.encode("utf-8"), b"\r\n", ]) def add_file(name: str, path_str: str) -> None: path = Path(path_str) chunks.extend([ f"--{boundary}\r\n".encode("ascii"), ( f'Content-Disposition: form-data; name="{name}"; ' f'filename="{path.name}"\r\n' ).encode("utf-8"), b"Content-Type: image/jpeg\r\n\r\n", path.read_bytes(), b"\r\n", ]) add_field("metadata", json.dumps(job.metadata, ensure_ascii=True), "application/json") add_file("label_image", job.label_image_path) if job.gaylord_image_path is not None: add_file("gaylord_image", job.gaylord_image_path) chunks.append(f"--{boundary}--\r\n".encode("ascii")) return b"".join(chunks), f"multipart/form-data; boundary={boundary}" def parse_window_rect(value: str | None) -> tuple[int, int, int, int] | None: if value is None: return None parts = [part.strip() for part in str(value).split(",")] if len(parts) != 4: return None try: x, y, w, h = [int(part) for part in parts] except ValueError: return None if w <= 0 or h <= 0: return None return x, y, w, h def apply_window_layout(args) -> None: if not args.window_layout_enabled: return layout = { "flywms navigate": args.navigate_window, "flywms comandi": args.commands_window, "flywms snapshot": args.snapshot_window, "flywms etichetta": args.label_window, } for name, value in layout.items(): rect = parse_window_rect(value) if rect is None: log(f"Layout finestra ignorato per {name}: {value}") continue x, y, w, h = rect cv2.resizeWindow(name, w, h) cv2.moveWindow(name, x, y) def clip_box(x1: int, y1: int, x2: int, y2: int, w: int, h: int) -> tuple[int, int, int, int]: x1 = max(0, min(x1, w - 1)) y1 = max(0, min(y1, h - 1)) x2 = max(0, min(x2, w - 1)) y2 = max(0, min(y2, h - 1)) return x1, y1, x2, y2 def crop_with_padding( frame: np.ndarray, bbox: tuple[int, int, int, int], pad_ratio: float, ) -> np.ndarray: x1, y1, x2, y2 = bbox bw = x2 - x1 bh = y2 - y1 pad_x = int(max(0.0, pad_ratio) * bw) pad_y = int(max(0.0, pad_ratio) * bh) cx1, cy1, cx2, cy2 = clip_box( x1 - pad_x, y1 - pad_y, x2 + pad_x, y2 + pad_y, frame.shape[1], frame.shape[0], ) return frame[cy1:cy2, cx1:cx2].copy() def bbox_area(bbox: tuple[int, int, int, int]) -> int: x1, y1, x2, y2 = bbox return max(0, x2 - x1) * max(0, y2 - y1) def bbox_center(bbox: tuple[int, int, int, int]) -> tuple[float, float]: x1, y1, x2, y2 = bbox return (x1 + x2) * 0.5, (y1 + y2) * 0.5 def bbox_contains( outer: tuple[int, int, int, int], inner: tuple[int, int, int, int], ) -> bool: ox1, oy1, ox2, oy2 = outer ix1, iy1, ix2, iy2 = inner return ox1 <= ix1 and oy1 <= iy1 and ix2 <= ox2 and iy2 <= oy2 def find_label_inside_bbox( gaylord_bbox: tuple[int, int, int, int], labels: list[Detection], ) -> Detection | None: contained = [label for label in labels if bbox_contains(gaylord_bbox, label.bbox)] if not contained: return None if len(contained) == 1: return contained[0] # Multiple contained labels should not happen; choose a deterministic fallback. gx, gy = bbox_center(gaylord_bbox) return min( contained, key=lambda label: ( np.hypot(bbox_center(label.bbox)[0] - gx, bbox_center(label.bbox)[1] - gy), -label.confidence, ), ) def bbox_iou(a: tuple[int, int, int, int], b: tuple[int, int, int, int]) -> float: ax1, ay1, ax2, ay2 = a bx1, by1, bx2, by2 = b ix1 = max(ax1, bx1) iy1 = max(ay1, by1) ix2 = min(ax2, bx2) iy2 = min(ay2, by2) inter = bbox_area((ix1, iy1, ix2, iy2)) union = bbox_area(a) + bbox_area(b) - inter if union <= 0: return 0.0 return inter / float(union) def association_score( track_bbox: tuple[int, int, int, int], det_bbox: tuple[int, int, int, int], max_center_distance: float, ) -> float: iou = bbox_iou(track_bbox, det_bbox) tx, ty = bbox_center(track_bbox) dx, dy = bbox_center(det_bbox) center_dist = float(np.hypot(tx - dx, ty - dy)) center_similarity = max(0.0, 1.0 - center_dist / max_center_distance) return 0.70 * iou + 0.30 * center_similarity def resize_preview(frame: np.ndarray, max_width: int) -> np.ndarray: h, w = frame.shape[:2] if max_width <= 0 or w <= max_width: return frame scale = max_width / float(w) return cuda_resize(frame, (int(w * scale), int(h * scale)), interpolation=cv2.INTER_LINEAR) def draw_navigation_debug( frame: np.ndarray, tracks: list[Track], args, labels: list[Detection] | None = None, movement_arrow: tuple[tuple[int, int], tuple[int, int]] | None = None, ) -> np.ndarray: labels = labels or [] display = frame.copy() h, w = display.shape[:2] center_x = int(w * 0.5) tol = int(w * args.center_tolerance_ratio) y_min = int(h * args.usable_y_min_ratio) y_max = int(h * args.usable_y_max_ratio) cv2.rectangle(display, (center_x - tol, y_min), (center_x + tol, y_max), (255, 255, 0), 4) cv2.line(display, (center_x, 0), (center_x, h), (255, 255, 0), 3) cv2.line(display, (0, y_min), (w, y_min), (100, 100, 100), 2) cv2.line(display, (0, y_max), (w, y_max), (100, 100, 100), 2) for track in tracks: x1, y1, x2, y2 = track.bbox color = state_color(track.state) thickness = 8 if track.state == "centered" else 5 cv2.rectangle(display, (x1, y1), (x2, y2), color, thickness) cx, cy = bbox_center(track.bbox) cv2.circle(display, (int(cx), int(cy)), 12, color, -1) cv2.circle(display, (int(cx), int(cy)), 18, (0, 0, 0), 3) for label in labels: x1, y1, x2, y2 = label.bbox cv2.rectangle(display, (x1, y1), (x2, y2), (0, 170, 255), 4) lx, ly = bbox_center(label.bbox) cv2.circle(display, (int(lx), int(ly)), 8, (0, 170, 255), -1) if movement_arrow is not None: start, end = movement_arrow cv2.arrowedLine(display, start, end, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.12) cv2.arrowedLine(display, end, start, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.12) return resize_preview(display, args.preview_width) def draw_commands_window(command_lines: list[str], motion_text: str) -> np.ndarray: lines = command_lines if command_lines else ["Nessun comando generato"] visible_lines = lines[:14] arrow_mode = command_arrow_mode(lines) arrow_h = 210 if arrow_mode else 0 canvas_h = max(340, 84 + len(visible_lines) * 34 + arrow_h) canvas = np.full((canvas_h, 980, 3), 245, dtype=np.uint8) cv2.putText( canvas, "COMANDI NAVIGAZIONE", (24, 42), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 0), 2, cv2.LINE_AA, ) cv2.putText( canvas, motion_text, (24, 76), cv2.FONT_HERSHEY_SIMPLEX, 0.80, (120, 0, 120), 2, cv2.LINE_AA, ) y = 122 for idx, line in enumerate(visible_lines): color = (0, 0, 180) if idx == 0 else (0, 90, 0) cv2.putText( canvas, line, (24, y), cv2.FONT_HERSHEY_SIMPLEX, 0.82, color, 2, cv2.LINE_AA, ) y += 36 if arrow_mode: draw_command_arrow(canvas, arrow_mode, y + 16) return canvas def build_observer_telemetry( frame_id: int, timestamp: float, capture_pose: dict[str, object] | None, processed_frames: int, navigator: NavigationController, tracks: list[Track], gaylords: list[Detection], labels: list[Detection], run_yolo: bool, last_yolo_ms: float, yolo_cycles: int, start_time: float, video_fps: float, preview_fps: float, yolo_target_fps: float, yolo_mode: str, ) -> dict[str, object]: elapsed = max(time.perf_counter() - start_time, 0.001) active = sum(1 for t in tracks if t.missed == 0) return { "frame_id": int(frame_id), "processed_frames": int(processed_frames), "timestamp": float(timestamp), "capture_pose": dict(capture_pose or {}), "command_text": navigator.last_command_text, "command_lines": list(navigator.last_command_lines), "motion_text": navigator.motion_text, "snapshot_counter": int(navigator.snapshot_counter), "det_count": int(len(gaylords)), "label_count": int(len(labels)), "track_count": int(len(tracks)), "active_track_count": int(active), "run_yolo": bool(run_yolo), "yolo_mode": str(yolo_mode), "last_yolo_ms": float(last_yolo_ms), "loop_fps": float(processed_frames / elapsed), "yolo_fps": float(yolo_cycles / elapsed), "video_fps": float(video_fps), "preview_fps": float(preview_fps), "yolo_target_fps": float(yolo_target_fps), } def command_arrow_mode(lines: list[str]) -> tuple[str, float, float] | None: for line in lines: if line.startswith("CENTRA_ETICHETTA"): return "to_label", *parse_command_delta(line) if line.startswith("RITORNA_CENTRO_GAYLORD"): dx, dy = parse_command_delta(line) return "to_gaylord", -dx, -dy return None def parse_command_delta(line: str) -> tuple[float, float]: dx = 0.0 dy = 0.0 for part in line.split(): if part.startswith("dx="): dx = float(part[3:].replace("px", "")) elif part.startswith("dy="): dy = float(part[3:].replace("px", "")) return dx, dy def draw_command_arrow(canvas: np.ndarray, arrow_info: tuple[str, float, float], y0: int) -> None: mode, dx, dy = arrow_info h, w = canvas.shape[:2] y0 = min(max(y0, 110), h - 160) center = (w // 2 - 120, y0 + 130) northeast = (w // 2 + 150, y0 + 34) if mode == "to_label": start, end = center, northeast title = "MOVIMENTO VERSO ETICHETTA" start_label = "CENTRO GAYLORD" end_label = "ETICHETTA" else: start, end = northeast, center title = "RITORNO AL CENTRO GAYLORD" start_label = "ETICHETTA" end_label = "CENTRO GAYLORD" cv2.rectangle(canvas, (24, y0), (w - 24, min(h - 18, y0 + 170)), (232, 232, 232), -1) cv2.rectangle(canvas, (24, y0), (w - 24, min(h - 18, y0 + 170)), (120, 120, 120), 2) cv2.putText(canvas, title, (44, y0 + 34), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 0), 2, cv2.LINE_AA) cv2.putText(canvas, f"DX ORIZZONTALE: {dx:+.0f} px", (44, y0 + 72), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 180), 2, cv2.LINE_AA) cv2.putText(canvas, f"DY VERTICALE: {dy:+.0f} px", (44, y0 + 108), cv2.FONT_HERSHEY_SIMPLEX, 0.86, (0, 0, 180), 2, cv2.LINE_AA) cv2.circle(canvas, center, 16, (70, 70, 70), -1) cv2.circle(canvas, northeast, 16, (0, 120, 255), -1) cv2.arrowedLine(canvas, start, end, (0, 0, 0), 12, cv2.LINE_AA, tipLength=0.22) cv2.putText(canvas, start_label, (start[0] - 115, start[1] + 42), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (0, 0, 0), 2, cv2.LINE_AA) cv2.putText(canvas, end_label, (end[0] - 78, max(24, end[1] - 24)), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (0, 0, 0), 2, cv2.LINE_AA) def apply_flash(frame: np.ndarray, alpha: float) -> np.ndarray: flash = np.full_like(frame, 255) alpha = min(max(alpha, 0.0), 1.0) return cv2.addWeighted(frame, 1.0 - alpha, flash, alpha, 0.0) def estimate_motion_from_tracks(tracks: list[Track], min_pixels: float) -> str: deltas: list[tuple[float, float]] = [] for track in tracks: if track.missed != 0 or len(track.center_history) < 2: continue x0, y0 = track.center_history[-2] x1, y1 = track.center_history[-1] deltas.append((x1 - x0, y1 - y0)) if not deltas: return "MOTO: n/d" dx = sum(delta[0] for delta in deltas) / len(deltas) dy = sum(delta[1] for delta in deltas) / len(deltas) abs_dx = abs(dx) abs_dy = abs(dy) if abs_dx < min_pixels and abs_dy < min_pixels: direction = "stabile" elif abs_dx >= abs_dy: direction = "destra" if dx > 0 else "sinistra" else: direction = "giu" if dy > 0 else "su" return f"MOTO: {direction} dx={dx:+.1f}px dy={dy:+.1f}px tracks={len(deltas)}" def state_color(state: str) -> tuple[int, int, int]: if state == "centered": return (0, 255, 255) if state == "snapshotted": return (255, 0, 255) if state == "candidate": return (0, 255, 0) if state == "exiting": return (0, 140, 255) return (255, 255, 255) def main() -> int: args = parse_args() if args.benchmark_mode: args.no_display = True args.window_layout_enabled = False args.realtime_playback = True args.preview_fps = args.benchmark_preview_fps if args.perf_log_path == "tempistiche.txt": args.perf_log_path = "tempistiche-benchmark.txt" log( f"Profilo benchmark attivo: no_display=true " f"preview_fps={format_fps_value(args.preview_fps)} " f"log={args.perf_log_path}" ) if args.observer_enabled: if not args.no_display: log("Observer esterno attivo: disabilito la UI locale integrata") args.no_display = True args.window_layout_enabled = False require_file(args.weights, "modello Ultralytics") detector = UltralyticsDetector(args.weights, args.ultralytics_device, args.yolo_half) log(f"Classi modello: {detector.classes}") log(f"OpenCV CUDA: {'attivo' if OPENCV_CUDA_AVAILABLE else 'non disponibile'}") log(f"YOLO half precision: {'attiva' if detector.half else 'disattiva'}") log("Nota tracker: questa versione usa tracking geometrico interno; ByteTrack/BoT-SORT restano candidati per confronto successivo.") cap, source_name = open_capture(args.video) if not cap.isOpened(): log(f"ERRORE: impossibile aprire sorgente video: {source_name}") return 1 video_fps = cap.get(cv2.CAP_PROP_FPS) total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) or 0) video_duration_sec = (total_frames / float(video_fps)) if video_fps and video_fps > 0 and total_frames > 0 else 0.0 preview_fps = args.preview_fps if args.preview_fps and args.preview_fps > 0 else video_fps if args.preview_fps and args.preview_fps > 0 and (args.video is None or str(args.video).isdigit()): cap.set(cv2.CAP_PROP_FPS, float(args.preview_fps)) frame_delay = 1.0 / preview_fps if args.realtime_playback and preview_fps and preview_fps > 1 else 0.0 log( f"FPS sorgente={format_fps_value(video_fps)} " f"preview_target={format_fps_value(preview_fps)} " f"yolo_target={format_fps_value(args.yolo_fps)}" ) if args.adaptive_yolo_enabled: log( "YOLO adattivo:" f" idle={format_fps_value(args.idle_yolo_fps)}" f" tracking={format_fps_value(args.tracking_yolo_fps)}" f" critical={format_fps_value(args.critical_yolo_fps)}" ) if video_duration_sec > 0: log( f"Durata nominale video={video_duration_sec:.2f}s " f"({format_seconds(video_duration_sec)})" ) perf_writer = PerfLogWriter( args.perf_log_path, args.perf_log_flush_interval_sec, args.perf_log_flush_lines, ) log(f"Log tempistiche: {perf_writer.path.resolve()}") perf_writer.write( "ts\tframe_id\trun_yolo\tread_ms\tyolo_ms\ttrack_ms\tdraw_ms\tui_ms\t" "snapshot_pause_ms\twms_wait_ms\tloop_ms\tloop_fps\tyolo_real_fps\t" "src_fps\tpreview_target\tyolo_target\tdet\tlabels\ttracks\tactive\t" "snapshots\tcommand" ) tracker = LightweightTracker( max_missed=args.max_track_missed, min_match_score=args.min_match_score, max_center_distance_ratio=args.max_center_distance_ratio, ) navigator = NavigationController(args) wms_client = WmsAsyncClient(args) observer = ObserverPublisher(args) capture_worker = CaptureWorker(cap, args, float(video_fps or 0.0), frame_delay, total_frames) capture_worker.start() if not args.no_display: cv2.namedWindow("flywms navigate", cv2.WINDOW_NORMAL) cv2.namedWindow("flywms snapshot", cv2.WINDOW_NORMAL) cv2.namedWindow("flywms etichetta", cv2.WINDOW_NORMAL) cv2.namedWindow("flywms comandi", cv2.WINDOW_NORMAL) apply_window_layout(args) frame_id = 0 processed_frames = 0 start_time = time.perf_counter() last_stats = start_time yolo_total_ms = 0.0 yolo_cycles = 0 total_wms_wait_ms = 0.0 next_yolo_time = start_time last_yolo_ms = 0.0 current_yolo_mode = "fixed" observer_preview_interval = 1.0 / args.observer_preview_fps if args.observer_preview_fps and args.observer_preview_fps > 0 else 0.0 observer_telemetry_interval = 1.0 / args.observer_telemetry_fps if args.observer_telemetry_fps and args.observer_telemetry_fps > 0 else 0.0 last_observer_preview = 0.0 last_observer_telemetry = 0.0 gaylords: list[Detection] = [] labels: list[Detection] = [] tracks: list[Track] = [] def publish_observer_state(captured: CapturedFrame, run_yolo_flag: bool, force: bool = False) -> None: nonlocal last_observer_preview, last_observer_telemetry if not args.observer_enabled: return now = time.perf_counter() if force or observer_telemetry_interval <= 0 or now - last_observer_telemetry >= observer_telemetry_interval: observer.publish_telemetry( build_observer_telemetry( frame_id=captured.frame_id, timestamp=captured.timestamp, capture_pose=captured.pose, processed_frames=processed_frames, navigator=navigator, tracks=tracks, gaylords=gaylords, labels=labels, run_yolo=run_yolo_flag, last_yolo_ms=last_yolo_ms, yolo_cycles=yolo_cycles, start_time=start_time, video_fps=float(video_fps or 0.0), preview_fps=float(preview_fps or 0.0), yolo_target_fps=float(args.yolo_fps or 0.0), yolo_mode=current_yolo_mode, ) ) last_observer_telemetry = now if force or observer_preview_interval <= 0 or now - last_observer_preview >= observer_preview_interval: preview_frame = draw_navigation_debug( captured.frame, tracks, args, labels, navigator.label_movement_arrow, ) preview_frame = resize_preview(preview_frame, args.observer_preview_width) observer.publish_preview("navigate", preview_frame, captured.frame_id, captured.timestamp, args.observer_jpeg_quality) if navigator.last_ocr_payload_frame is not None: observer.publish_preview( "snapshot", resize_preview(navigator.last_ocr_payload_frame, args.observer_preview_width), captured.frame_id, captured.timestamp, args.observer_jpeg_quality, ) if navigator.last_label_payload_frame is not None: observer.publish_preview( "label", resize_preview(navigator.last_label_payload_frame, args.observer_preview_width), captured.frame_id, captured.timestamp, args.observer_jpeg_quality, ) last_observer_preview = now try: while True: frame_loop_start = time.perf_counter() read_ms = 0.0 track_ms = 0.0 draw_ms = 0.0 ui_ms = 0.0 snapshot_pause_ms = 0.0 wms_wait_ms = 0.0 try: captured = capture_worker.get(timeout=2.0) except queue.Empty: continue if captured is None: log("Fine stream") break frame = captured.frame frame_id = captured.frame_id timestamp = captured.timestamp read_ms = captured.read_ms processed_frames += 1 if args.max_frames > 0 and frame_id > args.max_frames: log(f"Raggiunto --max-frames={args.max_frames}") break new_snapshots: list[NavigationSnapshot] = [] current_yolo_fps, current_yolo_mode = choose_adaptive_yolo_fps(args, tracks) yolo_interval = 1.0 / current_yolo_fps if current_yolo_fps and current_yolo_fps > 0 else 0.0 run_yolo = yolo_interval <= 0 or timestamp >= next_yolo_time if run_yolo: next_yolo_time = timestamp + yolo_interval detections, last_yolo_ms = detector.detect(frame, args.min_confidence, args.input_size) yolo_total_ms += last_yolo_ms yolo_cycles += 1 gaylords = [ det for det in detections if det.class_name.strip().lower() == args.target_class.strip().lower() ] labels = [ det for det in detections if det.class_name.strip().lower() == args.label_class.strip().lower() ] track_t0 = time.perf_counter() tracks = tracker.update(gaylords, frame_id, frame.shape[1]) if args.motion_report_interval > 0 and yolo_cycles % args.motion_report_interval == 0: navigator.set_motion_text( estimate_motion_from_tracks(tracks, args.motion_min_pixels) ) for track in tracks: if track.missed == 0: snapshot = navigator.process_track(track, captured, labels) if snapshot is not None: new_snapshots.append(snapshot) track_ms = (time.perf_counter() - track_t0) * 1000.0 if args.no_display and new_snapshots: if args.observer_enabled: for snapshot in new_snapshots: navigator.set_label_sequence_phase(snapshot, "move") publish_observer_state(captured, run_yolo, force=True) if args.label_move_sec > 0: pause_t0 = time.perf_counter() time.sleep(args.label_move_sec) snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0 navigator.set_label_sequence_phase(snapshot, "stabilize") publish_observer_state(captured, run_yolo, force=True) if args.label_stabilization_sec > 0: pause_t0 = time.perf_counter() time.sleep(args.label_stabilization_sec) snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0 navigator.set_label_sequence_phase(snapshot, "capture") publish_observer_state(captured, run_yolo, force=True) pause_t0 = time.perf_counter() time.sleep(0.5) snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0 request_id = wms_client.submit(snapshot) if args.wms_enabled else None navigator.set_label_sequence_phase(snapshot, "return") publish_observer_state(captured, run_yolo, force=True) if args.label_return_sec > 0: pause_t0 = time.perf_counter() time.sleep(args.label_return_sec) snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0 navigator.set_label_sequence_phase(snapshot, "wait_wms") publish_observer_state(captured, run_yolo, force=True) if args.wms_enabled: wait_t0 = time.perf_counter() result = wms_client.wait_for_result(request_id, args.remote_ack_timeout_sec) wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 navigator.apply_wms_result(result, snapshot) else: if args.remote_ack_timeout_sec > 0: wait_t0 = time.perf_counter() time.sleep(args.remote_ack_timeout_sec) wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 navigator.simulate_remote_response(snapshot) publish_observer_state(captured, run_yolo, force=True) navigator.label_movement_arrow = None else: sequence_sec = ( args.label_move_sec + args.label_stabilization_sec + args.label_return_sec ) if sequence_sec > 0: pause_t0 = time.perf_counter() time.sleep(sequence_sec) snapshot_pause_ms += (time.perf_counter() - pause_t0) * 1000.0 for snapshot in new_snapshots: if args.wms_enabled: request_id = wms_client.submit(snapshot) wait_t0 = time.perf_counter() result = wms_client.wait_for_result(request_id, args.remote_ack_timeout_sec) wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 navigator.apply_wms_result(result, snapshot) else: if args.remote_ack_timeout_sec > 0: wait_t0 = time.perf_counter() time.sleep(args.remote_ack_timeout_sec) wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 navigator.simulate_remote_response(snapshot) now = time.perf_counter() if now - last_stats >= args.stats_interval: elapsed = max(now - start_time, 0.001) avg_yolo = yolo_total_ms / max(yolo_cycles, 1) active = sum(1 for t in tracks if t.missed == 0) log( f"fps={processed_frames / elapsed:.1f} yolo_fps={yolo_cycles / elapsed:.1f} " f"yolo_mode={current_yolo_mode} " f"avg_yolo={avg_yolo:.1f}ms det={len(gaylords)} labels={len(labels)} " f"tracks={len(tracks)} active={active} " f"snapshots={navigator.snapshot_counter} {navigator.motion_text}" ) if args.debug_tracks: for track in tracks: cx, cy = bbox_center(track.bbox) area_ratio = bbox_area(track.bbox) / float(frame.shape[0] * frame.shape[1]) log( f" track={track.id} state={track.state} hits={track.hits} " f"missed={track.missed} center=({cx:.0f},{cy:.0f}) " f"area={area_ratio:.3f} trend={track.area_trend():+.2f} " f"reason={track.last_candidate_reason}" ) last_stats = now if args.observer_enabled: publish_observer_state(captured, run_yolo, force=False) if not args.no_display: draw_t0 = time.perf_counter() display = draw_navigation_debug( frame, tracks, args, labels, navigator.label_movement_arrow, ) draw_ms += (time.perf_counter() - draw_t0) * 1000.0 ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", display) if navigator.last_ocr_payload_frame is not None: snapshot_display = resize_preview(navigator.last_ocr_payload_frame, args.preview_width) cv2.imshow("flywms snapshot", snapshot_display) if navigator.last_label_payload_frame is not None and not new_snapshots: label_display = resize_preview(navigator.last_label_payload_frame, args.preview_width) cv2.imshow("flywms etichetta", label_display) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 if new_snapshots: stop_requested = False for snapshot in new_snapshots: navigator.set_label_sequence_phase(snapshot, "move") ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", display) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 wait_t0 = time.perf_counter() key = cv2.waitKey(max(1, int(args.label_move_sec * 1000))) & 0xFF snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True break navigator.set_label_sequence_phase(snapshot, "stabilize") ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", display) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 wait_t0 = time.perf_counter() key = cv2.waitKey(max(1, int(args.label_stabilization_sec * 1000))) & 0xFF snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True break navigator.set_label_sequence_phase(snapshot, "capture") flash_display = apply_flash(display, args.flash_alpha) ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", flash_display) if navigator.last_label_payload_frame is not None: flash_label = apply_flash( resize_preview(navigator.last_label_payload_frame, args.preview_width), args.flash_alpha, ) cv2.imshow("flywms etichetta", flash_label) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 wait_t0 = time.perf_counter() key = cv2.waitKey(500) & 0xFF snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True break request_id = wms_client.submit(snapshot) if args.wms_enabled else None navigator.set_label_sequence_phase(snapshot, "return") ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", display) if navigator.last_label_payload_frame is not None: cv2.imshow( "flywms etichetta", resize_preview(navigator.last_label_payload_frame, args.preview_width), ) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 wait_t0 = time.perf_counter() key = cv2.waitKey(max(1, int(args.label_return_sec * 1000))) & 0xFF snapshot_pause_ms += (time.perf_counter() - wait_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True break navigator.set_label_sequence_phase(snapshot, "wait_wms") ui_t0 = time.perf_counter() cv2.imshow("flywms navigate", display) if navigator.last_label_payload_frame is not None: cv2.imshow( "flywms etichetta", resize_preview(navigator.last_label_payload_frame, args.preview_width), ) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) ui_ms += (time.perf_counter() - ui_t0) * 1000.0 wait_deadline = time.perf_counter() + max(0.0, args.remote_ack_timeout_sec) wms_result: WmsResult | None = None if args.wms_enabled: wait_t0 = time.perf_counter() while time.perf_counter() <= wait_deadline: wms_result = wms_client.wait_for_result(request_id, 0.05) key = cv2.waitKey(1) & 0xFF if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True break if wms_result is not None: break wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 else: wait_t0 = time.perf_counter() key = cv2.waitKey(max(1, int(args.remote_ack_timeout_sec * 1000))) & 0xFF wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") stop_requested = True if stop_requested: break if args.wms_enabled: navigator.apply_wms_result(wms_result, snapshot) else: navigator.simulate_remote_response(snapshot) cv2.imshow( "flywms comandi", draw_commands_window(navigator.last_command_lines, navigator.motion_text), ) navigator.label_movement_arrow = None if stop_requested: break ui_t0 = time.perf_counter() key = cv2.waitKey(1) & 0xFF ui_ms += (time.perf_counter() - ui_t0) * 1000.0 if key in (27, ord("q")): log("Interrotto da tastiera") break loop_end = time.perf_counter() elapsed = max(loop_end - start_time, 0.001) loop_ms = (loop_end - frame_loop_start) * 1000.0 total_wms_wait_ms += wms_wait_ms active = sum(1 for t in tracks if t.missed == 0) perf_writer.write( f"{time.strftime('%Y-%m-%d %H:%M:%S')}\t{frame_id}\t{int(run_yolo)}\t" f"{read_ms:.3f}\t{last_yolo_ms if run_yolo else 0.0:.3f}\t{track_ms:.3f}\t" f"{draw_ms:.3f}\t{ui_ms:.3f}\t{snapshot_pause_ms:.3f}\t{wms_wait_ms:.3f}\t" f"{loop_ms:.3f}\t{processed_frames / elapsed:.3f}\t{yolo_cycles / elapsed:.3f}\t" f"{format_fps_value(video_fps)}\t{format_fps_value(preview_fps)}\t{format_fps_value(args.yolo_fps)}\t" f"{len(gaylords)}\t{len(labels)}\t{len(tracks)}\t{active}\t{navigator.snapshot_counter}\t" f"{json.dumps(navigator.last_command_text, ensure_ascii=True)}" ) finally: capture_worker.close() cap.release() wms_client.close() observer.close() perf_writer.close() if not args.no_display: cv2.destroyAllWindows() total_demo_sec = max(time.perf_counter() - start_time, 0.0) net_demo_sec = max(total_demo_sec - (total_wms_wait_ms / 1000.0), 0.0) if video_duration_sec > 0: delta_total_sec = total_demo_sec - video_duration_sec delta_net_sec = net_demo_sec - video_duration_sec ratio_total_pct = ((total_demo_sec / video_duration_sec) - 1.0) * 100.0 ratio_net_pct = ((net_demo_sec / video_duration_sec) - 1.0) * 100.0 log( "Durata demo:" f" totale={total_demo_sec:.2f}s ({format_seconds(total_demo_sec)})" f" netto_senza_wms={net_demo_sec:.2f}s ({format_seconds(net_demo_sec)})" ) log( "Confronto video/demo:" f" video={video_duration_sec:.2f}s ({format_seconds(video_duration_sec)})" f" delta_totale={delta_total_sec:+.2f}s ({ratio_total_pct:+.1f}%)" f" delta_netto_senza_wms={delta_net_sec:+.2f}s ({ratio_net_pct:+.1f}%)" ) log( f"Attesa WMS totale={total_wms_wait_ms / 1000.0:.2f}s " f"({format_seconds(total_wms_wait_ms / 1000.0)})" ) log(f"Snapshot salvati in: {Path(args.snapshot_output_dir).resolve()}") return 0 if __name__ == "__main__": raise SystemExit(main())