administrator/flywms
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

External observer with OpenCV UI baseline

Browse Source
This commit is contained in:
administrator
2026-06-03 15:28:27 +02:00
parent f728524ee6
commit e86c05a885
9 changed files with 1375 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

596
flywms_navigation.py
View File

@@ -1,7 +1,9 @@
import argparse
import base64
import configparser
import json
import queue
import socket
import sys
import threading
import time
@@ -71,6 +73,7 @@ class Detection:
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]
@@ -137,6 +140,7 @@ 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]
@@ -148,6 +152,16 @@ class NavigationSnapshot:
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
@@ -350,6 +364,124 @@ class WmsAsyncClient:
)
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."""
@@ -431,12 +563,13 @@ class NavigationController:
def process_track(
self,
track: Track,
frame: np.ndarray,
frame_id: int,
timestamp: float,
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)
@@ -459,6 +592,7 @@ class NavigationController:
candidate = CandidateSnapshot(
frame_id=frame_id,
timestamp=timestamp,
capture_pose=dict(captured.pose),
frame=frame.copy(),
bbox=track.bbox,
label_bbox=label.bbox,
@@ -623,6 +757,7 @@ class NavigationController:
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,
@@ -725,10 +860,7 @@ class NavigationController:
"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,
},
"drone_pose_simulated": dict(snapshot.capture_pose),
"track_id": snapshot.track_id,
"gaylord_bbox": list(snapshot.bbox),
"label_bbox": list(snapshot.label_bbox),
@@ -887,6 +1019,14 @@ def parse_args():
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")
@@ -901,6 +1041,18 @@ def parse_args():
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")
@@ -935,7 +1087,7 @@ def load_navigation_config(path_str: str) -> dict[str, object]:
"min_area_trend": -0.35,
"snapshot_window_frames": 1,
"snapshot_output_dir": "navigate_snapshots",
"remote_ack_timeout_sec": 2.0,
"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",
@@ -953,6 +1105,10 @@ def load_navigation_config(path_str: str) -> dict[str, object]:
"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",
@@ -963,6 +1119,13 @@ def load_navigation_config(path_str: str) -> dict[str, object]:
"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",
@@ -1010,6 +1173,35 @@ def format_fps_value(value: float | int | None) -> str:
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)
@@ -1039,6 +1231,117 @@ class PerfLogWriter:
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():
@@ -1324,6 +1627,50 @@ def draw_commands_window(command_lines: list[str], motion_text: str) -> np.ndarr
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"):
@@ -1433,6 +1780,11 @@ def main() -> int:
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)
@@ -1447,16 +1799,29 @@ def main() -> int:
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
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)}"
)
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,
@@ -1476,6 +1841,9 @@ def main() -> int:
)
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)
@@ -1485,17 +1853,78 @@ def main() -> int:
apply_window_layout(args)
frame_id = 0
processed_frames = 0
start_time = time.perf_counter()
last_stats = start_time
last_loop_end = 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()
@@ -1505,26 +1934,25 @@ def main() -> int:
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:
try:
captured = capture_worker.get(timeout=2.0)
except queue.Empty:
continue
if captured is None:
log("Fine stream")
break
frame_id += 1
timestamp = time.perf_counter()
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
@@ -1548,33 +1976,80 @@ def main() -> int:
)
for track in tracks:
if track.missed == 0:
snapshot = navigator.process_track(track, frame, frame_id, timestamp, labels)
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:
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:
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()
time.sleep(args.remote_ack_timeout_sec)
result = wms_client.wait_for_result(request_id, args.remote_ack_timeout_sec)
wms_wait_ms += (time.perf_counter() - wait_t0) * 1000.0
navigator.simulate_remote_response(snapshot)
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:
@@ -1582,7 +2057,8 @@ def main() -> int:
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"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}"
@@ -1599,6 +2075,9 @@ def main() -> int:
)
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(
@@ -1763,23 +2242,48 @@ def main() -> int:
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{frame_id / elapsed:.3f}\t{yolo_cycles / elapsed:.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