# ============================================================ # 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. # =============================================================