|
|
# ============================================================
|
|
|
# intercept_fsm.py
|
|
|
# Модуль 3: Finite State Machine фаз перехвата
|
|
|
#
|
|
|
# Состояния:
|
|
|
# SEARCH — нет цели, полное сканирование
|
|
|
# ACQUIRE — цель обнаружена, набираем подтверждений
|
|
|
# TRACK — стабильный трек, удержание / сближение
|
|
|
# INTERCEPT — активная атака, PN-наведение
|
|
|
# TERMINAL — финальная фаза (<1.5 сек до контакта)
|
|
|
# LOST — потеря цели в ходе перехвата, попытка реакквизиции
|
|
|
#
|
|
|
# FSM управляет:
|
|
|
# - какой закон наведения использовать
|
|
|
# - throttle bias
|
|
|
# - разрешение на переключение трека
|
|
|
# - параметры детектора (ROI / fullscan)
|
|
|
# ============================================================
|
|
|
|
|
|
import time
|
|
|
import numpy as np
|
|
|
|
|
|
from config import *
|
|
|
from config_intercept import *
|
|
|
from helpers import clamp
|
|
|
|
|
|
import cv2
|
|
|
|
|
|
|
|
|
class InterceptPhase:
|
|
|
"""Enum-like для фаз."""
|
|
|
SEARCH = "SEARCH"
|
|
|
ACQUIRE = "ACQUIRE"
|
|
|
TRACK = "TRACK"
|
|
|
INTERCEPT = "INTERCEPT"
|
|
|
TERMINAL = "TERMINAL"
|
|
|
LOST = "LOST"
|
|
|
|
|
|
|
|
|
class InterceptFSM:
|
|
|
"""
|
|
|
Конечный автомат фаз перехвата.
|
|
|
|
|
|
Определяет текущую фазу на основе:
|
|
|
- состояния трекера (confirmed, miss_streak, hit_streak)
|
|
|
- оценки дальности (range_m)
|
|
|
- tau (time-to-contact)
|
|
|
- уверенности (confidence)
|
|
|
|
|
|
Каждая фаза определяет набор параметров для guidance, throttle и трекера.
|
|
|
"""
|
|
|
|
|
|
def __init__(self):
|
|
|
self.enabled = bool(INTERCEPT_FSM_ENABLE)
|
|
|
self.phase = InterceptPhase.SEARCH
|
|
|
self.prev_phase = InterceptPhase.SEARCH
|
|
|
self.phase_start_time = time.perf_counter()
|
|
|
self.phase_frame_count = 0
|
|
|
|
|
|
# Счётчики для переходов
|
|
|
self._acquire_frames = 0
|
|
|
self._lost_frames = 0
|
|
|
self._terminal_locked = False
|
|
|
|
|
|
# Замороженные команды для TERMINAL lock
|
|
|
self._frozen_cmd = None
|
|
|
|
|
|
# Лог переходов
|
|
|
self.transitions = []
|
|
|
|
|
|
def reset(self):
|
|
|
self._transition_to(InterceptPhase.SEARCH)
|
|
|
self._acquire_frames = 0
|
|
|
self._lost_frames = 0
|
|
|
self._terminal_locked = False
|
|
|
self._frozen_cmd = None
|
|
|
self.transitions.clear()
|
|
|
|
|
|
def update(self, confirmed, hit_streak, miss_streak,
|
|
|
range_state, guidance_state, frame_id):
|
|
|
"""
|
|
|
Обновить FSM. Вызывается каждый кадр.
|
|
|
|
|
|
Args:
|
|
|
confirmed: bool — цель подтверждена трекером
|
|
|
hit_streak: int — кол-во последовательных hit'ов
|
|
|
miss_streak: int — кол-во последовательных miss'ов
|
|
|
range_state: dict из RangeEstimator.update()
|
|
|
guidance_state: dict из guidance.py или PN
|
|
|
frame_id: int
|
|
|
|
|
|
Returns:
|
|
|
dict — параметры текущей фазы для всех модулей
|
|
|
"""
|
|
|
if not self.enabled:
|
|
|
return self._default_params()
|
|
|
|
|
|
range_m = range_state.get("range_m") if range_state else None
|
|
|
tau = range_state.get("tau", float('inf')) if range_state else float('inf')
|
|
|
closing_vel = range_state.get("closing_vel", 0.0) if range_state else 0.0
|
|
|
range_conf = range_state.get("confidence", 0.0) if range_state else 0.0
|
|
|
range_phase = range_state.get("phase", "far") if range_state else "far"
|
|
|
|
|
|
self.phase_frame_count += 1
|
|
|
|
|
|
# ─── Переходы ───────────────────────────────────────────
|
|
|
|
|
|
if self.phase == InterceptPhase.SEARCH:
|
|
|
if confirmed:
|
|
|
self._transition_to(InterceptPhase.ACQUIRE)
|
|
|
|
|
|
elif self.phase == InterceptPhase.ACQUIRE:
|
|
|
if not confirmed and miss_streak > 3:
|
|
|
self._transition_to(InterceptPhase.SEARCH)
|
|
|
elif hit_streak >= int(FSM_TRACK_CONFIRM_HITS):
|
|
|
self._transition_to(InterceptPhase.TRACK)
|
|
|
elif self.phase_frame_count > int(FSM_ACQUIRE_TIMEOUT):
|
|
|
self._transition_to(InterceptPhase.SEARCH)
|
|
|
|
|
|
elif self.phase == InterceptPhase.TRACK:
|
|
|
if not confirmed and miss_streak > int(FSM_LOST_TIMEOUT):
|
|
|
self._transition_to(InterceptPhase.LOST)
|
|
|
elif self._should_intercept(range_m, tau, closing_vel, range_conf):
|
|
|
self._transition_to(InterceptPhase.INTERCEPT)
|
|
|
|
|
|
elif self.phase == InterceptPhase.INTERCEPT:
|
|
|
if not confirmed and miss_streak > int(FSM_LOST_TIMEOUT) // 2:
|
|
|
self._transition_to(InterceptPhase.LOST)
|
|
|
elif self._should_terminal(range_m, tau, range_conf):
|
|
|
self._transition_to(InterceptPhase.TERMINAL)
|
|
|
elif self._should_back_to_track(range_m, closing_vel):
|
|
|
self._transition_to(InterceptPhase.TRACK)
|
|
|
|
|
|
elif self.phase == InterceptPhase.TERMINAL:
|
|
|
# В TERMINAL мы уже не откатываемся — либо контакт,
|
|
|
# либо пролёт → LOST
|
|
|
if not confirmed and miss_streak > 10:
|
|
|
# Вероятно пролетели мимо или столкнулись
|
|
|
self._transition_to(InterceptPhase.SEARCH)
|
|
|
elif tau > float(TAU_WARN_SEC) * 2.0 and miss_streak > 3:
|
|
|
# Tau вырос обратно — пролёт
|
|
|
self._transition_to(InterceptPhase.LOST)
|
|
|
|
|
|
elif self.phase == InterceptPhase.LOST:
|
|
|
if confirmed and hit_streak >= 2:
|
|
|
# Реакквизиция — возвращаемся в TRACK или INTERCEPT
|
|
|
if self._should_intercept(range_m, tau, closing_vel, range_conf):
|
|
|
self._transition_to(InterceptPhase.INTERCEPT)
|
|
|
else:
|
|
|
self._transition_to(InterceptPhase.TRACK)
|
|
|
elif self.phase_frame_count > int(FSM_LOST_RECOVER_TIMEOUT):
|
|
|
self._transition_to(InterceptPhase.SEARCH)
|
|
|
|
|
|
# ─── Параметры текущей фазы ─────────────────────────────
|
|
|
return self._build_params(range_m, tau, closing_vel,
|
|
|
guidance_state, confirmed)
|
|
|
|
|
|
# ─── Условия переходов ───────────────────────────────────────
|
|
|
|
|
|
def _should_intercept(self, range_m, tau, closing_vel, conf):
|
|
|
"""TRACK → INTERCEPT"""
|
|
|
if range_m is not None and range_m <= float(FSM_INTERCEPT_RANGE_M):
|
|
|
return True
|
|
|
if closing_vel > float(PN_MIN_CLOSING_VEL) and tau < float(TAU_WARN_SEC) * 2.0:
|
|
|
return True
|
|
|
return False
|
|
|
|
|
|
def _should_terminal(self, range_m, tau, conf):
|
|
|
"""INTERCEPT → TERMINAL"""
|
|
|
if tau <= float(FSM_TERMINAL_TAU_SEC) and tau > 0:
|
|
|
return True
|
|
|
if range_m is not None and range_m <= float(FSM_TERMINAL_RANGE_M):
|
|
|
return True
|
|
|
return False
|
|
|
|
|
|
def _should_back_to_track(self, range_m, closing_vel):
|
|
|
"""INTERCEPT → TRACK (цель удаляется)"""
|
|
|
if closing_vel < -2.0 and self.phase_frame_count > 30:
|
|
|
return True
|
|
|
if range_m is not None and range_m > float(FSM_INTERCEPT_RANGE_M) * 1.5:
|
|
|
return True
|
|
|
return False
|
|
|
|
|
|
# ─── Параметры фаз ──────────────────────────────────────────
|
|
|
|
|
|
def _build_params(self, range_m, tau, closing_vel,
|
|
|
guidance_state, confirmed):
|
|
|
"""Собирает параметры для всех модулей на основе текущей фазы."""
|
|
|
|
|
|
params = {
|
|
|
"phase": self.phase,
|
|
|
"prev_phase": self.prev_phase,
|
|
|
"phase_frame_count": self.phase_frame_count,
|
|
|
"throttle_bias": 0.5,
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": True,
|
|
|
"allow_target_switch": True,
|
|
|
"force_fullscan": False,
|
|
|
"det_every_override": None,
|
|
|
"yolo_enabled": True,
|
|
|
"klt_only": False,
|
|
|
"lock_commands": False,
|
|
|
"frozen_cmd": None,
|
|
|
"range_m": range_m,
|
|
|
"tau": tau,
|
|
|
"closing_vel": closing_vel,
|
|
|
}
|
|
|
|
|
|
if self.phase == InterceptPhase.SEARCH:
|
|
|
params.update({
|
|
|
"throttle_bias": 0.4,
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": False,
|
|
|
"allow_target_switch": True,
|
|
|
"force_fullscan": True,
|
|
|
})
|
|
|
|
|
|
elif self.phase == InterceptPhase.ACQUIRE:
|
|
|
params.update({
|
|
|
"throttle_bias": 0.45,
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": True,
|
|
|
"allow_target_switch": True,
|
|
|
"det_every_override": 2,
|
|
|
})
|
|
|
|
|
|
elif self.phase == InterceptPhase.TRACK:
|
|
|
params.update({
|
|
|
"throttle_bias": float(FSM_TRACK_THROTTLE_BIAS),
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": True,
|
|
|
"allow_target_switch": False,
|
|
|
})
|
|
|
|
|
|
elif self.phase == InterceptPhase.INTERCEPT:
|
|
|
params.update({
|
|
|
"throttle_bias": float(FSM_INTERCEPT_THROTTLE_BIAS),
|
|
|
"use_pn": True,
|
|
|
"use_screen_guidance": False,
|
|
|
"allow_target_switch": False,
|
|
|
"det_every_override": 1, # максимальная частота детекции
|
|
|
})
|
|
|
|
|
|
elif self.phase == InterceptPhase.TERMINAL:
|
|
|
should_lock = (
|
|
|
bool(FSM_TERMINAL_LOCK_COMMANDS)
|
|
|
and tau <= float(FSM_TERMINAL_LOCK_TAU_SEC)
|
|
|
)
|
|
|
|
|
|
if should_lock and not self._terminal_locked:
|
|
|
# Замораживаем команды
|
|
|
self._terminal_locked = True
|
|
|
if guidance_state is not None:
|
|
|
self._frozen_cmd = {
|
|
|
"steer_x": guidance_state.get("steer_x", 0.0),
|
|
|
"steer_y": guidance_state.get("steer_y", 0.0),
|
|
|
"cmd_x": guidance_state.get("cmd_x", 0.0),
|
|
|
"cmd_y": guidance_state.get("cmd_y", 0.0),
|
|
|
}
|
|
|
|
|
|
params.update({
|
|
|
"throttle_bias": float(FSM_TERMINAL_THROTTLE),
|
|
|
"use_pn": not should_lock,
|
|
|
"use_screen_guidance": False,
|
|
|
"allow_target_switch": False,
|
|
|
"yolo_enabled": not bool(FSM_TERMINAL_USE_KLT_ONLY),
|
|
|
"klt_only": bool(FSM_TERMINAL_USE_KLT_ONLY),
|
|
|
"lock_commands": should_lock,
|
|
|
"frozen_cmd": self._frozen_cmd if should_lock else None,
|
|
|
"det_every_override": 1,
|
|
|
})
|
|
|
|
|
|
elif self.phase == InterceptPhase.LOST:
|
|
|
params.update({
|
|
|
"throttle_bias": 0.5,
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": False,
|
|
|
"allow_target_switch": True,
|
|
|
"force_fullscan": True,
|
|
|
})
|
|
|
|
|
|
return params
|
|
|
|
|
|
def _default_params(self):
|
|
|
"""Параметры по умолчанию когда FSM отключена."""
|
|
|
return {
|
|
|
"phase": "DISABLED",
|
|
|
"prev_phase": "DISABLED",
|
|
|
"phase_frame_count": 0,
|
|
|
"throttle_bias": 0.5,
|
|
|
"use_pn": False,
|
|
|
"use_screen_guidance": True,
|
|
|
"allow_target_switch": True,
|
|
|
"force_fullscan": False,
|
|
|
"det_every_override": None,
|
|
|
"yolo_enabled": True,
|
|
|
"klt_only": False,
|
|
|
"lock_commands": False,
|
|
|
"frozen_cmd": None,
|
|
|
"range_m": None,
|
|
|
"tau": float('inf'),
|
|
|
"closing_vel": 0.0,
|
|
|
}
|
|
|
|
|
|
def _transition_to(self, new_phase):
|
|
|
"""Выполнить переход в новую фазу."""
|
|
|
if new_phase == self.phase:
|
|
|
return
|
|
|
|
|
|
old_phase = self.phase
|
|
|
self.prev_phase = old_phase
|
|
|
self.phase = new_phase
|
|
|
self.phase_start_time = time.perf_counter()
|
|
|
self.phase_frame_count = 0
|
|
|
|
|
|
# Сброс фазо-специфичных счётчиков
|
|
|
if new_phase == InterceptPhase.TERMINAL:
|
|
|
self._terminal_locked = False
|
|
|
self._frozen_cmd = None
|
|
|
elif new_phase == InterceptPhase.SEARCH:
|
|
|
self._acquire_frames = 0
|
|
|
self._lost_frames = 0
|
|
|
self._terminal_locked = False
|
|
|
self._frozen_cmd = None
|
|
|
|
|
|
self.transitions.append({
|
|
|
"time": time.perf_counter(),
|
|
|
"from": old_phase,
|
|
|
"to": new_phase,
|
|
|
})
|
|
|
|
|
|
# Ограничиваем лог
|
|
|
if len(self.transitions) > 200:
|
|
|
self.transitions = self.transitions[-100:]
|
|
|
|
|
|
# ─── Status / Draw ───────────────────────────────────────────
|
|
|
|
|
|
def status_line(self):
|
|
|
if not self.enabled:
|
|
|
return "Intercept FSM disabled"
|
|
|
return f"Intercept FSM ready: phase={self.phase}"
|
|
|
|
|
|
def draw_overlay(self, frame_bgr, params):
|
|
|
"""Рисует текущую фазу перехвата."""
|
|
|
if not self.enabled or not DRAW_INTERCEPT_FSM:
|
|
|
return
|
|
|
|
|
|
phase = params.get("phase", "?")
|
|
|
tau = params.get("tau", float('inf'))
|
|
|
range_m = params.get("range_m")
|
|
|
throttle = params.get("throttle_bias", 0.5)
|
|
|
|
|
|
# Цвета по фазе
|
|
|
phase_colors = {
|
|
|
InterceptPhase.SEARCH: (150, 150, 150),
|
|
|
InterceptPhase.ACQUIRE: (0, 200, 255),
|
|
|
InterceptPhase.TRACK: (255, 200, 0),
|
|
|
InterceptPhase.INTERCEPT: (0, 128, 255),
|
|
|
InterceptPhase.TERMINAL: (0, 0, 255),
|
|
|
InterceptPhase.LOST: (128, 0, 128),
|
|
|
}
|
|
|
color = phase_colors.get(phase, (180, 180, 180))
|
|
|
|
|
|
# Фон для фазы (полоска сверху)
|
|
|
h, w = frame_bgr.shape[:2]
|
|
|
if phase == InterceptPhase.TERMINAL:
|
|
|
# Мигающая красная полоска
|
|
|
if (self.phase_frame_count // 4) % 2 == 0:
|
|
|
cv2.rectangle(frame_bgr, (0, 0), (w, 6), (0, 0, 255), -1)
|
|
|
|
|
|
# Текст фазы
|
|
|
range_str = f"{range_m:.1f}m" if range_m is not None else "---"
|
|
|
tau_str = f"{tau:.2f}s" if tau < 100 else "---"
|
|
|
|
|
|
txt = (f"FSM: {phase} R={range_str} "
|
|
|
f"tau={tau_str} thr={throttle:.0%}")
|
|
|
cv2.putText(frame_bgr, txt, (20, 290),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.60, color, 2)
|
|
|
|
|
|
# Индикатор use_pn
|
|
|
if params.get("use_pn"):
|
|
|
cv2.putText(frame_bgr, "PN", (w - 60, 40),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 128), 2)
|
|
|
|
|
|
if params.get("lock_commands"):
|
|
|
cv2.putText(frame_bgr, "CMD LOCK", (w - 160, 70),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|