administrator/flywms
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
f728524ee6eb8fa7ab47c2ad497b45b237ab5890
flywms/flywms_navigation.py
administrator f728524ee6 pipeline in linea single thread
2026-05-19 08:52:44 +02:00

1789 lines
73 KiB
Python
Raw Blame History

import argparse
import configparser
import json
import queue
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
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
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 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 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,
frame: np.ndarray,
frame_id: int,
timestamp: float,
labels: list[Detection] | None = None,
) -> NavigationSnapshot | None:
labels = labels or []
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,
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,
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": {
"mode": "linear_shelf_scan",
"position_label": snapshot.simulated_position,
},
"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("--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("--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": 2.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,
"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,
"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}"
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 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 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}"
)
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)
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
yolo_interval = 1.0 / args.yolo_fps if args.yolo_fps and args.yolo_fps > 0 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)}"
)
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)
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
start_time = time.perf_counter()
last_stats = start_time
last_loop_end = start_time
yolo_total_ms = 0.0
yolo_cycles = 0
next_yolo_time = start_time
last_yolo_ms = 0.0
gaylords: list[Detection] = []
labels: list[Detection] = []
tracks: list[Track] = []
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
if frame_delay > 0:
now = time.perf_counter()
sleep_for = frame_delay - (now - last_loop_end)
if sleep_for > 0:
time.sleep(sleep_for)
last_loop_end = time.perf_counter()
read_t0 = time.perf_counter()
ok, frame = cap.read()
read_ms = (time.perf_counter() - read_t0) * 1000.0
if not ok:
log("Fine stream")
break
frame_id += 1
timestamp = time.perf_counter()
if args.max_frames > 0 and frame_id > args.max_frames:
log(f"Raggiunto --max-frames={args.max_frames}")
break
new_snapshots: list[NavigationSnapshot] = []
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, frame, frame_id, timestamp, 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:
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={frame_id / elapsed:.1f} yolo_fps={yolo_cycles / elapsed:.1f} "
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 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
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{frame_id / 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:
cap.release()
wms_client.close()
perf_writer.close()
if not args.no_display:
cv2.destroyAllWindows()
log(f"Snapshot salvati in: {Path(args.snapshot_output_dir).resolve()}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
Reference in New Issue View Git Blame Copy Permalink