|
|
# ============================================================
|
|
|
# operator_hud.py
|
|
|
# Операторский HUD для ручного перехвата
|
|
|
#
|
|
|
# Показывает пилоту:
|
|
|
# - Направление к цели (стрелка "рули сюда")
|
|
|
# - Расстояние и время до контакта (tau)
|
|
|
# - Индикатор качества захвата (lock quality)
|
|
|
# - Прицельный маркер с lead-предсказанием
|
|
|
# - Зону поражения (kill zone)
|
|
|
# - Звуковой сигнал при сближении (опционально)
|
|
|
#
|
|
|
# Интегрируется в main.py одним вызовом draw().
|
|
|
# Не требует автопилота, моторов, IMU.
|
|
|
# ============================================================
|
|
|
|
|
|
import math
|
|
|
import time
|
|
|
import numpy as np
|
|
|
import cv2
|
|
|
|
|
|
from config import *
|
|
|
from helpers import clamp, box_center, box_wh, box_area, clip_box
|
|
|
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
# КОНФИГУРАЦИЯ
|
|
|
# ─────────────────────────────────────────────────────────────
|
|
|
|
|
|
# Камера (для оценки дальности)
|
|
|
HUD_FOCAL_PX = 315.0 # focal length в пикселях (откалибровать!)
|
|
|
HUD_TARGET_SIZE_M = 0.27 # типичный размер цели (м)
|
|
|
|
|
|
# Фазы сближения (по tau, секунды)
|
|
|
HUD_PHASE_FAR_TAU = 999.0 # далеко, просто трек
|
|
|
HUD_PHASE_APPROACH_TAU = 5.0 # заход на цель
|
|
|
HUD_PHASE_ATTACK_TAU = 2.5 # атака, финальный бросок
|
|
|
HUD_PHASE_TERMINAL_TAU = 1.0 # терминальная фаза, не отвлекаться
|
|
|
|
|
|
# Lead-предсказание для прицельного маркера
|
|
|
HUD_LEAD_SEC = 0.20 # упреждение (секунды вперёд)
|
|
|
HUD_LEAD_MAX_PX = 120.0 # максимум пикселей lead
|
|
|
|
|
|
# Kill zone — при попадании цели в этот круг, пилот "на цели"
|
|
|
HUD_KILL_ZONE_RADIUS_NORM = 0.06 # доля от ширины кадра
|
|
|
HUD_ON_TARGET_MIN_FRAMES = 3 # стабильно на цели N кадров
|
|
|
|
|
|
# Звук (через системный beep, работает на Linux/Windows)
|
|
|
HUD_AUDIO_ENABLE = False # включить звуковые подсказки
|
|
|
HUD_AUDIO_APPROACH_HZ = 2.0 # частота бипов при approach
|
|
|
HUD_AUDIO_ATTACK_HZ = 6.0 # частота при attack
|
|
|
HUD_AUDIO_TERMINAL_HZ = 15.0 # частота при terminal (почти непрерывно)
|
|
|
|
|
|
# Элементы HUD
|
|
|
HUD_SHOW_STEER_ARROW = True # стрелка "рули сюда"
|
|
|
HUD_SHOW_RANGE = True # дальность
|
|
|
HUD_SHOW_TAU = True # время до контакта
|
|
|
HUD_SHOW_LOCK_QUALITY = True # полоска качества лока
|
|
|
HUD_SHOW_LEAD_MARKER = True # прицел с упреждением
|
|
|
HUD_SHOW_KILL_ZONE = True # зона поражения
|
|
|
HUD_SHOW_SPEED_BAR = True # скорость цели
|
|
|
HUD_SHOW_PHASE_BANNER = True # баннер фазы вверху экрана
|
|
|
|
|
|
# Мигание при terminal
|
|
|
HUD_TERMINAL_BLINK_RATE = 8 # Гц мигания рамки
|
|
|
|
|
|
# Tau: минимальный рост площади для оценки
|
|
|
HUD_TAU_MIN_AREA_RATE = 0.5
|
|
|
|
|
|
|
|
|
class OperatorHUD:
|
|
|
"""
|
|
|
Всё что нужно пилоту для ручного перехвата.
|
|
|
Один вызов draw() в каждом кадре — рисует поверх frame_orig.
|
|
|
"""
|
|
|
|
|
|
def __init__(self):
|
|
|
# Оценка дальности
|
|
|
self._area_hist = [] # [(timestamp, area), ...]
|
|
|
self._area_hist_max = 12
|
|
|
self._range_m = None
|
|
|
self._tau = float('inf')
|
|
|
self._closing_vel = 0.0
|
|
|
|
|
|
# Фаза
|
|
|
self.phase = "SEARCH" # SEARCH | TRACK | APPROACH | ATTACK | TERMINAL
|
|
|
self._phase_start = time.perf_counter()
|
|
|
|
|
|
# On-target счётчик
|
|
|
self._on_target_count = 0
|
|
|
self.on_target = False
|
|
|
|
|
|
# Аудио
|
|
|
self._last_beep_time = 0.0
|
|
|
|
|
|
# Lead smoothing
|
|
|
self._smooth_lead_x = 0.0
|
|
|
self._smooth_lead_y = 0.0
|
|
|
|
|
|
def update(self, locked_box_eff, pred_box_eff, kf,
|
|
|
confirmed, miss_streak, hit_streak,
|
|
|
klt_quality, frame_ts, dt, ew, eh):
|
|
|
"""
|
|
|
Обновляет внутреннее состояние HUD. Вызывать каждый кадр.
|
|
|
|
|
|
Args:
|
|
|
locked_box_eff: текущий locked bbox (effective pixels) или None
|
|
|
pred_box_eff: Kalman-predicted bbox или None
|
|
|
kf: Kalman8D или IMMFilter
|
|
|
confirmed: трек подтверждён
|
|
|
miss_streak: пропуски подряд
|
|
|
hit_streak: хиты подряд
|
|
|
klt_quality: качество KLT (0..1)
|
|
|
frame_ts: timestamp кадра
|
|
|
dt: дельта времени
|
|
|
ew, eh: размеры effective frame
|
|
|
|
|
|
Returns:
|
|
|
dict с метриками для отображения
|
|
|
"""
|
|
|
ref_box = locked_box_eff if locked_box_eff is not None else pred_box_eff
|
|
|
|
|
|
# ─── Range + Tau ────────────────────────────────────────
|
|
|
if ref_box is not None:
|
|
|
w, h = box_wh(ref_box)
|
|
|
apparent = float(max(w, h))
|
|
|
area = float(w * h)
|
|
|
|
|
|
# Дальность
|
|
|
if apparent >= 3.0 and HUD_FOCAL_PX > 10.0:
|
|
|
self._range_m = (HUD_TARGET_SIZE_M * HUD_FOCAL_PX) / apparent
|
|
|
else:
|
|
|
self._range_m = None
|
|
|
|
|
|
# Tau
|
|
|
self._area_hist.append((frame_ts, area))
|
|
|
if len(self._area_hist) > self._area_hist_max:
|
|
|
self._area_hist = self._area_hist[-self._area_hist_max:]
|
|
|
self._tau = self._compute_tau()
|
|
|
self._closing_vel = self._compute_closing_vel()
|
|
|
else:
|
|
|
self._range_m = None
|
|
|
self._tau = float('inf')
|
|
|
self._closing_vel = 0.0
|
|
|
self._area_hist.clear()
|
|
|
|
|
|
# ─── Phase ──────────────────────────────────────────────
|
|
|
new_phase = self._classify_phase(confirmed, miss_streak)
|
|
|
if new_phase != self.phase:
|
|
|
self.phase = new_phase
|
|
|
self._phase_start = time.perf_counter()
|
|
|
|
|
|
# ─── On-target ──────────────────────────────────────────
|
|
|
self._update_on_target(ref_box, ew, eh)
|
|
|
|
|
|
# ─── Lead marker ────────────────────────────────────────
|
|
|
lead_x, lead_y = 0.0, 0.0
|
|
|
if kf.initialized and ref_box is not None:
|
|
|
vx = float(kf.x[4, 0])
|
|
|
vy = float(kf.x[5, 0])
|
|
|
lead_x = vx * HUD_LEAD_SEC
|
|
|
lead_y = vy * HUD_LEAD_SEC
|
|
|
mag = float(np.hypot(lead_x, lead_y))
|
|
|
if mag > HUD_LEAD_MAX_PX:
|
|
|
lead_x *= HUD_LEAD_MAX_PX / mag
|
|
|
lead_y *= HUD_LEAD_MAX_PX / mag
|
|
|
|
|
|
self._smooth_lead_x = 0.6 * self._smooth_lead_x + 0.4 * lead_x
|
|
|
self._smooth_lead_y = 0.6 * self._smooth_lead_y + 0.4 * lead_y
|
|
|
|
|
|
# ─── Audio ──────────────────────────────────────────────
|
|
|
if HUD_AUDIO_ENABLE:
|
|
|
self._do_audio(frame_ts)
|
|
|
|
|
|
# ─── Lock quality ───────────────────────────────────────
|
|
|
lock_q = self._compute_lock_quality(
|
|
|
confirmed, miss_streak, hit_streak, klt_quality
|
|
|
)
|
|
|
|
|
|
# ─── Speed ──────────────────────────────────────────────
|
|
|
speed_px = 0.0
|
|
|
if kf.initialized:
|
|
|
speed_px = float(np.hypot(kf.x[4, 0], kf.x[5, 0]))
|
|
|
|
|
|
return {
|
|
|
"phase": self.phase,
|
|
|
"range_m": self._range_m,
|
|
|
"tau": self._tau,
|
|
|
"closing_vel": self._closing_vel,
|
|
|
"on_target": self.on_target,
|
|
|
"lock_quality": lock_q,
|
|
|
"lead_x": self._smooth_lead_x,
|
|
|
"lead_y": self._smooth_lead_y,
|
|
|
"speed_px": speed_px,
|
|
|
}
|
|
|
|
|
|
def draw(self, frame_orig, locked_box_eff, pred_box_eff, kf,
|
|
|
hud_state, sx, sy):
|
|
|
"""
|
|
|
Рисует весь HUD поверх frame_orig.
|
|
|
|
|
|
Args:
|
|
|
frame_orig: кадр для рисования (BGR)
|
|
|
locked_box_eff: locked bbox в effective pixels
|
|
|
pred_box_eff: predicted bbox в effective pixels
|
|
|
kf: Kalman/IMM (для скорости)
|
|
|
hud_state: dict из update()
|
|
|
sx, sy: масштаб effective → original
|
|
|
"""
|
|
|
oh, ow = frame_orig.shape[:2]
|
|
|
ref_box = locked_box_eff if locked_box_eff is not None else pred_box_eff
|
|
|
phase = hud_state["phase"]
|
|
|
|
|
|
# Цвета по фазе
|
|
|
phase_colors = {
|
|
|
"SEARCH": (150, 150, 150),
|
|
|
"TRACK": (0, 220, 255),
|
|
|
"APPROACH": (0, 165, 255),
|
|
|
"ATTACK": (0, 80, 255),
|
|
|
"TERMINAL": (0, 0, 255),
|
|
|
}
|
|
|
color = phase_colors.get(phase, (150, 150, 150))
|
|
|
|
|
|
# ─── Kill zone ──────────────────────────────────────────
|
|
|
if HUD_SHOW_KILL_ZONE:
|
|
|
self._draw_kill_zone(frame_orig, ow, oh, color, phase)
|
|
|
|
|
|
# ─── Стрелка наведения ──────────────────────────────────
|
|
|
if HUD_SHOW_STEER_ARROW and ref_box is not None:
|
|
|
self._draw_steer_arrow(frame_orig, ref_box, sx, sy, ow, oh, color)
|
|
|
|
|
|
# ─── Lead marker ────────────────────────────────────────
|
|
|
if HUD_SHOW_LEAD_MARKER and ref_box is not None:
|
|
|
self._draw_lead_marker(frame_orig, ref_box, hud_state, sx, sy, color)
|
|
|
|
|
|
# ─── Lock quality bar ───────────────────────────────────
|
|
|
if HUD_SHOW_LOCK_QUALITY:
|
|
|
self._draw_lock_quality(frame_orig, hud_state["lock_quality"],
|
|
|
ow, oh, color)
|
|
|
|
|
|
# ─── Speed bar ──────────────────────────────────────────
|
|
|
if HUD_SHOW_SPEED_BAR and ref_box is not None:
|
|
|
self._draw_speed_bar(frame_orig, hud_state["speed_px"],
|
|
|
ow, oh, color)
|
|
|
|
|
|
# ─── Range + Tau text ───────────────────────────────────
|
|
|
if HUD_SHOW_RANGE or HUD_SHOW_TAU:
|
|
|
self._draw_range_tau(frame_orig, hud_state, ow, oh, color)
|
|
|
|
|
|
# ─── Phase banner ───────────────────────────────────────
|
|
|
if HUD_SHOW_PHASE_BANNER:
|
|
|
self._draw_phase_banner(frame_orig, phase, hud_state, ow, oh, color)
|
|
|
|
|
|
# ─── Terminal blink ─────────────────────────────────────
|
|
|
if phase == "TERMINAL":
|
|
|
self._draw_terminal_blink(frame_orig, ow, oh)
|
|
|
|
|
|
# ─── On-target indicator ────────────────────────────────
|
|
|
if hud_state["on_target"]:
|
|
|
self._draw_on_target(frame_orig, ow, oh)
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────
|
|
|
# DRAW HELPERS
|
|
|
# ─────────────────────────────────────────────────────────
|
|
|
|
|
|
def _draw_kill_zone(self, frame, ow, oh, color, phase):
|
|
|
"""Кольцо kill zone в центре экрана."""
|
|
|
cx, cy = ow // 2, oh // 2
|
|
|
radius = int(float(ow) * HUD_KILL_ZONE_RADIUS_NORM)
|
|
|
|
|
|
if phase in ("ATTACK", "TERMINAL"):
|
|
|
# Заливка полупрозрачная
|
|
|
overlay = frame.copy()
|
|
|
cv2.circle(overlay, (cx, cy), radius, color, 2)
|
|
|
cv2.addWeighted(overlay, 0.5, frame, 0.5, 0, frame)
|
|
|
else:
|
|
|
cv2.circle(frame, (cx, cy), radius, (80, 80, 80), 1)
|
|
|
|
|
|
# Перекрестие
|
|
|
gap = 8
|
|
|
arm = radius + 12
|
|
|
cv2.line(frame, (cx - arm, cy), (cx - gap, cy), (80, 80, 80), 1)
|
|
|
cv2.line(frame, (cx + gap, cy), (cx + arm, cy), (80, 80, 80), 1)
|
|
|
cv2.line(frame, (cx, cy - arm), (cx, cy - gap), (80, 80, 80), 1)
|
|
|
cv2.line(frame, (cx, cy + gap), (cx, cy + arm), (80, 80, 80), 1)
|
|
|
|
|
|
def _draw_steer_arrow(self, frame, ref_box, sx, sy, ow, oh, color):
|
|
|
"""
|
|
|
Стрелка от центра экрана к цели — показывает оператору
|
|
|
КУДА РУЛИТЬ. Размер стрелки пропорционален ошибке.
|
|
|
Когда цель в центре — стрелка исчезает.
|
|
|
"""
|
|
|
center_eff = box_center(ref_box)
|
|
|
# → original coords
|
|
|
tx = float(center_eff[0]) / max(1e-6, sx)
|
|
|
ty = float(center_eff[1]) / max(1e-6, sy)
|
|
|
|
|
|
scr_cx = ow * 0.5
|
|
|
scr_cy = oh * 0.5
|
|
|
|
|
|
dx = tx - scr_cx
|
|
|
dy = ty - scr_cy
|
|
|
dist = float(np.hypot(dx, dy))
|
|
|
|
|
|
# Мёртвая зона: если цель почти в центре, не рисуем стрелку
|
|
|
deadzone = float(ow) * 0.03
|
|
|
if dist < deadzone:
|
|
|
return
|
|
|
|
|
|
# Нормализуем
|
|
|
ux = dx / dist
|
|
|
uy = dy / dist
|
|
|
|
|
|
# Длина стрелки: от 30 до 120px, пропорционально ошибке
|
|
|
max_arrow = min(120, ow // 6)
|
|
|
arrow_len = float(clamp(dist * 0.4, 30, max_arrow))
|
|
|
|
|
|
# Начало стрелки — на расстоянии от центра
|
|
|
start_r = 35
|
|
|
sx_a = int(scr_cx + ux * start_r)
|
|
|
sy_a = int(scr_cy + uy * start_r)
|
|
|
ex_a = int(scr_cx + ux * (start_r + arrow_len))
|
|
|
ey_a = int(scr_cy + uy * (start_r + arrow_len))
|
|
|
|
|
|
# Толщина зависит от фазы
|
|
|
thickness = 2
|
|
|
if self.phase in ("ATTACK", "TERMINAL"):
|
|
|
thickness = 3
|
|
|
|
|
|
cv2.arrowedLine(frame, (sx_a, sy_a), (ex_a, ey_a),
|
|
|
color, thickness, cv2.LINE_AA, tipLength=0.3)
|
|
|
|
|
|
def _draw_lead_marker(self, frame, ref_box, hud_state, sx, sy, color):
|
|
|
"""
|
|
|
Прицельный маркер с УПРЕЖДЕНИЕМ.
|
|
|
Показывает где цель БУДЕТ через HUD_LEAD_SEC секунд.
|
|
|
Оператор должен целиться сюда, а не в текущую позицию.
|
|
|
"""
|
|
|
center_eff = box_center(ref_box)
|
|
|
lead_x = hud_state["lead_x"]
|
|
|
lead_y = hud_state["lead_y"]
|
|
|
|
|
|
# Lead point в effective → original
|
|
|
lx = float(center_eff[0] + lead_x) / max(1e-6, sx)
|
|
|
ly = float(center_eff[1] + lead_y) / max(1e-6, sy)
|
|
|
lx = int(lx)
|
|
|
ly = int(ly)
|
|
|
|
|
|
# Ромб-маркер
|
|
|
size = 10
|
|
|
pts = np.array([
|
|
|
[lx, ly - size],
|
|
|
[lx + size, ly],
|
|
|
[lx, ly + size],
|
|
|
[lx - size, ly],
|
|
|
], dtype=np.int32)
|
|
|
cv2.polylines(frame, [pts], True, color, 2, cv2.LINE_AA)
|
|
|
|
|
|
# Линия от текущей позиции к lead
|
|
|
cx = int(float(center_eff[0]) / max(1e-6, sx))
|
|
|
cy = int(float(center_eff[1]) / max(1e-6, sy))
|
|
|
cv2.line(frame, (cx, cy), (lx, ly), color, 1, cv2.LINE_AA)
|
|
|
|
|
|
def _draw_lock_quality(self, frame, quality, ow, oh, color):
|
|
|
"""
|
|
|
Вертикальная полоска слева — качество захвата.
|
|
|
Зелёная = отличный лок, жёлтая = нормально, красная = теряем.
|
|
|
"""
|
|
|
bar_x = 12
|
|
|
bar_y = oh // 4
|
|
|
bar_h = oh // 2
|
|
|
bar_w = 8
|
|
|
|
|
|
# Фон
|
|
|
cv2.rectangle(frame, (bar_x, bar_y),
|
|
|
(bar_x + bar_w, bar_y + bar_h), (40, 40, 40), -1)
|
|
|
|
|
|
# Заполнение снизу вверх
|
|
|
fill_h = int(bar_h * quality)
|
|
|
fill_y = bar_y + bar_h - fill_h
|
|
|
|
|
|
if quality >= 0.7:
|
|
|
fill_color = (0, 200, 0)
|
|
|
elif quality >= 0.4:
|
|
|
fill_color = (0, 200, 200)
|
|
|
else:
|
|
|
fill_color = (0, 0, 200)
|
|
|
|
|
|
if fill_h > 0:
|
|
|
cv2.rectangle(frame, (bar_x, fill_y),
|
|
|
(bar_x + bar_w, bar_y + bar_h), fill_color, -1)
|
|
|
|
|
|
# Рамка
|
|
|
cv2.rectangle(frame, (bar_x, bar_y),
|
|
|
(bar_x + bar_w, bar_y + bar_h), (120, 120, 120), 1)
|
|
|
|
|
|
def _draw_speed_bar(self, frame, speed_px, ow, oh, color):
|
|
|
"""
|
|
|
Горизонтальная полоска внизу — скорость цели.
|
|
|
Помогает оператору предвидеть манёвры.
|
|
|
"""
|
|
|
bar_x = ow // 4
|
|
|
bar_y = oh - 30
|
|
|
bar_w = ow // 2
|
|
|
bar_h = 6
|
|
|
|
|
|
cv2.rectangle(frame, (bar_x, bar_y),
|
|
|
(bar_x + bar_w, bar_y + bar_h), (40, 40, 40), -1)
|
|
|
|
|
|
# Нормализуем скорость (0-200 px/s → 0-1)
|
|
|
norm_speed = float(clamp(speed_px / 200.0, 0.0, 1.0))
|
|
|
fill_w = int(bar_w * norm_speed)
|
|
|
|
|
|
if norm_speed < 0.3:
|
|
|
fill_color = (0, 200, 0)
|
|
|
elif norm_speed < 0.7:
|
|
|
fill_color = (0, 200, 200)
|
|
|
else:
|
|
|
fill_color = (0, 80, 255)
|
|
|
|
|
|
if fill_w > 0:
|
|
|
cv2.rectangle(frame, (bar_x, bar_y),
|
|
|
(bar_x + fill_w, bar_y + bar_h), fill_color, -1)
|
|
|
|
|
|
cv2.putText(frame, f"SPD", (bar_x - 40, bar_y + bar_h),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (120, 120, 120), 1)
|
|
|
|
|
|
def _draw_range_tau(self, frame, hud_state, ow, oh, color):
|
|
|
"""Дальность и time-to-contact — крупно, справа."""
|
|
|
range_m = hud_state["range_m"]
|
|
|
tau = hud_state["tau"]
|
|
|
closing = hud_state["closing_vel"]
|
|
|
|
|
|
x = ow - 200
|
|
|
y = oh // 2 - 30
|
|
|
|
|
|
if HUD_SHOW_RANGE and range_m is not None:
|
|
|
if range_m < 10:
|
|
|
range_txt = f"{range_m:.1f}m"
|
|
|
elif range_m < 100:
|
|
|
range_txt = f"{range_m:.0f}m"
|
|
|
else:
|
|
|
range_txt = f"{range_m:.0f}m"
|
|
|
|
|
|
font_scale = 1.0 if range_m < 20 else 0.75
|
|
|
cv2.putText(frame, range_txt, (x, y),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, font_scale, color, 2)
|
|
|
y += 35
|
|
|
|
|
|
if HUD_SHOW_TAU and tau < 30.0:
|
|
|
if tau < 2.0:
|
|
|
tau_txt = f"{tau:.1f}s"
|
|
|
tau_color = (0, 0, 255)
|
|
|
font_scale = 1.2
|
|
|
elif tau < 5.0:
|
|
|
tau_txt = f"{tau:.1f}s"
|
|
|
tau_color = (0, 80, 255)
|
|
|
font_scale = 0.9
|
|
|
else:
|
|
|
tau_txt = f"{tau:.0f}s"
|
|
|
tau_color = color
|
|
|
font_scale = 0.75
|
|
|
|
|
|
cv2.putText(frame, tau_txt, (x, y),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, font_scale, tau_color, 2)
|
|
|
y += 30
|
|
|
|
|
|
if closing > 0.5:
|
|
|
cv2.putText(frame, f"Vc {closing:.0f}m/s", (x, y),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 1)
|
|
|
|
|
|
def _draw_phase_banner(self, frame, phase, hud_state, ow, oh, color):
|
|
|
"""Баннер фазы в верхнем правом углу."""
|
|
|
labels = {
|
|
|
"SEARCH": "SEARCH",
|
|
|
"TRACK": "TRACK",
|
|
|
"APPROACH": "APPROACH",
|
|
|
"ATTACK": "ATTACK",
|
|
|
"TERMINAL": "COMMIT",
|
|
|
}
|
|
|
label = labels.get(phase, phase)
|
|
|
|
|
|
(tw, th), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.8, 2)
|
|
|
tx = ow - tw - 16
|
|
|
ty = 35
|
|
|
|
|
|
# Фон
|
|
|
pad = 6
|
|
|
cv2.rectangle(frame, (tx - pad, ty - th - pad),
|
|
|
(tx + tw + pad, ty + pad), (0, 0, 0), -1)
|
|
|
cv2.rectangle(frame, (tx - pad, ty - th - pad),
|
|
|
(tx + tw + pad, ty + pad), color, 2)
|
|
|
cv2.putText(frame, label, (tx, ty),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2)
|
|
|
|
|
|
def _draw_terminal_blink(self, frame, ow, oh):
|
|
|
"""Мигающая красная рамка в TERMINAL фазе."""
|
|
|
t = time.perf_counter()
|
|
|
cycle = int(t * HUD_TERMINAL_BLINK_RATE) % 2
|
|
|
if cycle == 0:
|
|
|
thick = 4
|
|
|
cv2.rectangle(frame, (0, 0), (ow - 1, oh - 1), (0, 0, 255), thick)
|
|
|
|
|
|
def _draw_on_target(self, frame, ow, oh):
|
|
|
"""Индикатор "цель в прицеле" — мигающий кружок."""
|
|
|
cx, cy = ow // 2, oh // 2
|
|
|
radius = int(float(ow) * HUD_KILL_ZONE_RADIUS_NORM)
|
|
|
|
|
|
t = time.perf_counter()
|
|
|
if int(t * 6) % 2 == 0:
|
|
|
cv2.circle(frame, (cx, cy), radius, (0, 255, 0), 3)
|
|
|
cv2.putText(frame, "ON TARGET", (cx - 60, cy + radius + 25),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 255, 0), 2)
|
|
|
|
|
|
# ─────────────────────────────────────────────────────────
|
|
|
# INTERNAL
|
|
|
# ─────────────────────────────────────────────────────────
|
|
|
|
|
|
def _compute_tau(self):
|
|
|
"""Time-to-contact из скорости роста площади."""
|
|
|
if len(self._area_hist) < 4:
|
|
|
return float('inf')
|
|
|
|
|
|
times = np.array([t for t, _ in self._area_hist], dtype=np.float64)
|
|
|
areas = np.array([a for _, a in self._area_hist], dtype=np.float64)
|
|
|
|
|
|
t0 = times[0]
|
|
|
times = times - t0
|
|
|
span = times[-1] - times[0]
|
|
|
if span < 0.05:
|
|
|
return float('inf')
|
|
|
|
|
|
# Линрег для dA/dt
|
|
|
n = len(times)
|
|
|
st = np.sum(times)
|
|
|
sa = np.sum(areas)
|
|
|
stt = np.sum(times * times)
|
|
|
sta = np.sum(times * areas)
|
|
|
denom = n * stt - st * st
|
|
|
if abs(denom) < 1e-12:
|
|
|
return float('inf')
|
|
|
|
|
|
area_rate = (n * sta - st * sa) / denom
|
|
|
|
|
|
if area_rate < HUD_TAU_MIN_AREA_RATE:
|
|
|
return float('inf')
|
|
|
|
|
|
current = float(areas[-1])
|
|
|
if current < 1.0:
|
|
|
return float('inf')
|
|
|
|
|
|
return max(0.0, current / (2.0 * area_rate))
|
|
|
|
|
|
def _compute_closing_vel(self):
|
|
|
"""Closing velocity из изменения дальности."""
|
|
|
if len(self._area_hist) < 4 or self._range_m is None:
|
|
|
return 0.0
|
|
|
|
|
|
# Грубая оценка: из tau и range
|
|
|
if self._tau > 0.1 and self._tau < 100.0 and self._range_m is not None:
|
|
|
return float(self._range_m / self._tau)
|
|
|
return 0.0
|
|
|
|
|
|
def _classify_phase(self, confirmed, miss_streak):
|
|
|
"""Определяет фазу для оператора."""
|
|
|
if not confirmed or miss_streak > 10:
|
|
|
return "SEARCH"
|
|
|
|
|
|
tau = self._tau
|
|
|
range_m = self._range_m
|
|
|
|
|
|
if tau <= HUD_PHASE_TERMINAL_TAU:
|
|
|
return "TERMINAL"
|
|
|
if tau <= HUD_PHASE_ATTACK_TAU:
|
|
|
return "ATTACK"
|
|
|
if tau <= HUD_PHASE_APPROACH_TAU:
|
|
|
return "APPROACH"
|
|
|
|
|
|
# Fallback по дальности
|
|
|
if range_m is not None:
|
|
|
if range_m < 8.0:
|
|
|
return "TERMINAL"
|
|
|
if range_m < 30.0:
|
|
|
return "ATTACK"
|
|
|
if range_m < 80.0:
|
|
|
return "APPROACH"
|
|
|
|
|
|
return "TRACK"
|
|
|
|
|
|
def _update_on_target(self, ref_box, ew, eh):
|
|
|
"""Проверяет, находится ли цель в kill zone."""
|
|
|
if ref_box is None:
|
|
|
self._on_target_count = 0
|
|
|
self.on_target = False
|
|
|
return
|
|
|
|
|
|
center = box_center(ref_box)
|
|
|
scr_cx = ew * 0.5
|
|
|
scr_cy = eh * 0.5
|
|
|
err = float(np.hypot(center[0] - scr_cx, center[1] - scr_cy))
|
|
|
threshold = float(ew) * HUD_KILL_ZONE_RADIUS_NORM
|
|
|
|
|
|
if err <= threshold:
|
|
|
self._on_target_count += 1
|
|
|
else:
|
|
|
self._on_target_count = max(0, self._on_target_count - 1)
|
|
|
|
|
|
self.on_target = (self._on_target_count >= HUD_ON_TARGET_MIN_FRAMES)
|
|
|
|
|
|
def _compute_lock_quality(self, confirmed, miss_streak, hit_streak,
|
|
|
klt_quality):
|
|
|
"""0..1 — интегральное качество захвата для оператора."""
|
|
|
if not confirmed:
|
|
|
return 0.0
|
|
|
|
|
|
q = 0.0
|
|
|
q += 0.35 * float(clamp(hit_streak / 10.0, 0.0, 1.0))
|
|
|
q += 0.30 * float(clamp(klt_quality, 0.0, 1.0))
|
|
|
q += 0.20 * float(clamp(1.0 - miss_streak / 8.0, 0.0, 1.0))
|
|
|
q += 0.15 # бонус за confirmed
|
|
|
return float(clamp(q, 0.0, 1.0))
|
|
|
|
|
|
def _do_audio(self, frame_ts):
|
|
|
"""Звуковой сигнал через системный beep."""
|
|
|
if self.phase == "SEARCH" or self.phase == "TRACK":
|
|
|
return
|
|
|
|
|
|
hz_map = {
|
|
|
"APPROACH": HUD_AUDIO_APPROACH_HZ,
|
|
|
"ATTACK": HUD_AUDIO_ATTACK_HZ,
|
|
|
"TERMINAL": HUD_AUDIO_TERMINAL_HZ,
|
|
|
}
|
|
|
beep_rate = hz_map.get(self.phase, 0.0)
|
|
|
if beep_rate <= 0:
|
|
|
return
|
|
|
|
|
|
interval = 1.0 / beep_rate
|
|
|
if (frame_ts - self._last_beep_time) >= interval:
|
|
|
self._last_beep_time = frame_ts
|
|
|
try:
|
|
|
# Кроссплатформенный beep
|
|
|
import sys
|
|
|
if sys.platform == "win32":
|
|
|
import winsound
|
|
|
freq = 800 if self.phase == "TERMINAL" else 600
|
|
|
winsound.Beep(freq, 30)
|
|
|
else:
|
|
|
# Linux: \a через stdout
|
|
|
sys.stdout.write('\a')
|
|
|
sys.stdout.flush()
|
|
|
except Exception:
|
|
|
pass
|
|
|
|
|
|
|
|
|
# =============================================================
|
|
|
# ИНТЕГРАЦИЯ В MAIN.PY
|
|
|
# =============================================================
|
|
|
#
|
|
|
# 1. Импорт:
|
|
|
# from operator_hud import OperatorHUD
|
|
|
#
|
|
|
# 2. Инициализация (после guidance_ctrl):
|
|
|
# hud = OperatorHUD()
|
|
|
#
|
|
|
# 3. В главном цикле, ПОСЛЕ guidance_state (перед отрисовкой):
|
|
|
#
|
|
|
# hud_state = hud.update(
|
|
|
# locked_box_eff=locked_box_eff,
|
|
|
# pred_box_eff=pred_box_eff,
|
|
|
# kf=kf,
|
|
|
# confirmed=confirmed,
|
|
|
# miss_streak=miss_streak,
|
|
|
# hit_streak=hit_streak,
|
|
|
# klt_quality=klt.quality,
|
|
|
# frame_ts=frame_ts,
|
|
|
# dt=dt,
|
|
|
# ew=ew,
|
|
|
# eh=eh,
|
|
|
# )
|
|
|
#
|
|
|
# 4. Отрисовка (ПОСЛЕ существующего draw, перед writer.write):
|
|
|
#
|
|
|
# hud.draw(
|
|
|
# frame_orig,
|
|
|
# locked_box_eff=locked_box_eff,
|
|
|
# pred_box_eff=pred_box_eff,
|
|
|
# kf=kf,
|
|
|
# hud_state=hud_state,
|
|
|
# sx=sx,
|
|
|
# sy=sy,
|
|
|
# )
|
|
|
#
|
|
|
# Всё. Четыре строки в main.py.
|
|
|
# =============================================================
|