|
|
|
|
@ -1,705 +0,0 @@
|
|
|
|
|
#!/usr/bin/env python3
|
|
|
|
|
"""
|
|
|
|
|
任務編排器 (MissionOrchestrator) —— ABC 對抗任務的階段狀態機。
|
|
|
|
|
|
|
|
|
|
角色：坐在既有 MissionExecutor 之上的「任務大腦」。偵察飛行由操作員用既有的
|
|
|
|
|
群組執行按鈕控制（A 組 GRID_SWEEP、B 組待機）；orchestrator 只做兩件事：
|
|
|
|
|
1. 被動監看「A↔敵機」距離，達 R_detect 持續 dwell → latch，提示操作員。
|
|
|
|
|
2. 操作員按「重編隊」→ 接管 A+B 四機，用「瞬時隊形」追移動敵機、足夠近時展開包圍。
|
|
|
|
|
|
|
|
|
|
追蹤採「瞬時隊形」而非路徑跟隨：每 tick 給每台一個目標點（此刻該站的隊形位置），
|
|
|
|
|
敵機移動就更新。避免了路徑跟隨在移動目標下 index 歸零/往後外推造成的往返振盪。
|
|
|
|
|
|
|
|
|
|
階段：IDLE → WATCH（監看偵測）→ DETECTED（latch，等確認）
|
|
|
|
|
→ PURSUE（4 機瞬時 leader-follow 朝敵，B 在後方 slot 自然追上會合）
|
|
|
|
|
→ ENCIRCLE（形心距敵 ≤ R_engage 展開包圍，圓心追敵；逃出 R_engage_exit 退回）
|
|
|
|
|
|
|
|
|
|
設計定案見 memory: design_mission_orchestrator。修改限定 src/GUI/（scope_gui_only）。
|
|
|
|
|
"""
|
|
|
|
|
import math
|
|
|
|
|
import time
|
|
|
|
|
from enum import Enum
|
|
|
|
|
|
|
|
|
|
from PyQt6.QtWidgets import (
|
|
|
|
|
QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton,
|
|
|
|
|
QDoubleSpinBox
|
|
|
|
|
)
|
|
|
|
|
from PyQt6.QtCore import Qt, QObject, pyqtSignal
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _log(level, message):
|
|
|
|
|
print(f"[{level}] {message}", flush=True)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _haversine(lat1, lon1, lat2, lon2):
|
|
|
|
|
"""兩經緯度間的水平地面距離 (m)。"""
|
|
|
|
|
R = 6371000.0
|
|
|
|
|
p1 = math.radians(lat1)
|
|
|
|
|
p2 = math.radians(lat2)
|
|
|
|
|
dphi = math.radians(lat2 - lat1)
|
|
|
|
|
dlam = math.radians(lon2 - lon1)
|
|
|
|
|
a = (math.sin(dphi / 2) ** 2
|
|
|
|
|
+ math.cos(p1) * math.cos(p2) * math.sin(dlam / 2) ** 2)
|
|
|
|
|
return 2 * R * math.asin(min(1.0, math.sqrt(a)))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _ll_to_m(lat, lon, ref_lat, ref_lon):
|
|
|
|
|
"""經緯度 → 以 ref 為原點的本地平面公尺 (x=東, y=北)。等距近似，小範圍夠用。"""
|
|
|
|
|
x = math.radians(lon - ref_lon) * 6371000.0 * math.cos(math.radians(ref_lat))
|
|
|
|
|
y = math.radians(lat - ref_lat) * 6371000.0
|
|
|
|
|
return x, y
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _m_to_ll(x, y, ref_lat, ref_lon):
|
|
|
|
|
"""本地平面公尺 → 經緯度。"""
|
|
|
|
|
lat = ref_lat + math.degrees(y / 6371000.0)
|
|
|
|
|
lon = ref_lon + math.degrees(x / (6371000.0 * math.cos(math.radians(ref_lat))))
|
|
|
|
|
return lat, lon
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 預設參數
|
|
|
|
|
DEFAULT_R_DETECT = 40.0 # 偵測範圍 (m)
|
|
|
|
|
DEFAULT_DETECT_DWELL = 1.0 # 偵測 dwell (s)
|
|
|
|
|
DEFAULT_R_ENGAGE = 15.0 # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍
|
|
|
|
|
DEFAULT_R_ENGAGE_EXIT = 20.0 # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis）
|
|
|
|
|
DEFAULT_REPLAN_MOVE_M = 2.0 # 敵機位移 ≥ 此值即重算（moving target）
|
|
|
|
|
DEFAULT_REPLAN_CAP_SEC = 1.0 # 重算週期上限 (s)
|
|
|
|
|
DEFAULT_CIRCLE_RADIUS = 10.0 # 包圍半徑 (m)
|
|
|
|
|
DEFAULT_LON_SPACING = 5.0 # 隊形縱向間距 (m)
|
|
|
|
|
DEFAULT_LAT_OFFSET = 3.0 # 隊形橫向錯開 (m)
|
|
|
|
|
DEFAULT_FORMATION_ALT = 10.0 # 隊形基準高度 (m，相對 home)
|
|
|
|
|
DEFAULT_ALT_LAYER_GAP = 6.0 # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2
|
|
|
|
|
DEFAULT_MIN_ALT_FLOOR = 5.0 # 相對 home 的最低安全高度 (m)：B 下層不得低於此值
|
|
|
|
|
DEFAULT_LEAD_TIME_CAP = 4.0 # 前置攔截 lead_time 上限 (s)
|
|
|
|
|
DEFAULT_FRIENDLY_SPEED = 5.0 # 友機假設水平速度 (m/s)：估 lead_time = gap/此值
|
|
|
|
|
DEFAULT_ENEMY_VEL_WINDOW = 1.0 # 敵速估計的 GPS 差分視窗 (s)
|
|
|
|
|
|
|
|
|
|
_NONE_LABEL = "（未選擇）"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class MissionPhase(Enum):
|
|
|
|
|
IDLE = "idle" # 未設定敵機/A 組
|
|
|
|
|
WATCH = "watch" # 監看偵測（偵察飛行由操作員的群組任務負責）
|
|
|
|
|
DETECTED = "detected" # 偵測 latch，等操作員確認重編隊
|
|
|
|
|
PURSUE = "pursue" # 4 機瞬時 leader-follow 朝敵機
|
|
|
|
|
ENCIRCLE = "encircle" # 4 機包圍敵機
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class MissionOrchestrator(QObject):
|
|
|
|
|
"""
|
|
|
|
|
階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）：
|
|
|
|
|
make_executor() -> MissionExecutor（complete_stops=False，hover-hold）
|
|
|
|
|
gps_provider() -> {drone_id: {'lat','lon','alt'}}
|
|
|
|
|
stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點）
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
phase_changed = pyqtSignal(str) # MissionPhase.value
|
|
|
|
|
detection_prompt = pyqtSignal(str) # 偵測提示（含最近友機/距離）
|
|
|
|
|
status_message = pyqtSignal(str)
|
|
|
|
|
|
|
|
|
|
def __init__(self, make_executor, gps_provider, stop_groups_fn=None,
|
|
|
|
|
r_detect=DEFAULT_R_DETECT,
|
|
|
|
|
detect_dwell_sec=DEFAULT_DETECT_DWELL,
|
|
|
|
|
r_engage=DEFAULT_R_ENGAGE,
|
|
|
|
|
r_engage_exit=DEFAULT_R_ENGAGE_EXIT,
|
|
|
|
|
replan_move_m=DEFAULT_REPLAN_MOVE_M,
|
|
|
|
|
replan_cap_sec=DEFAULT_REPLAN_CAP_SEC,
|
|
|
|
|
circle_radius=DEFAULT_CIRCLE_RADIUS,
|
|
|
|
|
lon_spacing=DEFAULT_LON_SPACING,
|
|
|
|
|
lat_offset=DEFAULT_LAT_OFFSET,
|
|
|
|
|
formation_alt=DEFAULT_FORMATION_ALT,
|
|
|
|
|
alt_layer_gap=DEFAULT_ALT_LAYER_GAP,
|
|
|
|
|
min_alt_floor=DEFAULT_MIN_ALT_FLOOR,
|
|
|
|
|
lead_time_cap_sec=DEFAULT_LEAD_TIME_CAP,
|
|
|
|
|
friendly_speed=DEFAULT_FRIENDLY_SPEED,
|
|
|
|
|
enemy_vel_window_sec=DEFAULT_ENEMY_VEL_WINDOW,
|
|
|
|
|
parent=None):
|
|
|
|
|
super().__init__(parent)
|
|
|
|
|
self._make_executor = make_executor
|
|
|
|
|
self._gps_provider = gps_provider
|
|
|
|
|
self._stop_groups_fn = stop_groups_fn
|
|
|
|
|
self.r_detect = float(r_detect)
|
|
|
|
|
self.detect_dwell_sec = float(detect_dwell_sec)
|
|
|
|
|
self.r_engage = float(r_engage)
|
|
|
|
|
self.r_engage_exit = float(r_engage_exit)
|
|
|
|
|
self.replan_move_m = float(replan_move_m)
|
|
|
|
|
self.replan_cap_sec = float(replan_cap_sec)
|
|
|
|
|
self.circle_radius = float(circle_radius)
|
|
|
|
|
self.lon_spacing = float(lon_spacing)
|
|
|
|
|
self.lat_offset = float(lat_offset)
|
|
|
|
|
self.formation_alt = float(formation_alt)
|
|
|
|
|
self.alt_layer_gap = float(alt_layer_gap)
|
|
|
|
|
self.min_alt_floor = float(min_alt_floor)
|
|
|
|
|
self.lead_time_cap_sec = float(lead_time_cap_sec)
|
|
|
|
|
self.friendly_speed = float(friendly_speed)
|
|
|
|
|
self.enemy_vel_window_sec = float(enemy_vel_window_sec)
|
|
|
|
|
|
|
|
|
|
self.phase = MissionPhase.IDLE
|
|
|
|
|
self.a_drone_ids = [] # 偵察組
|
|
|
|
|
self.b_drone_ids = [] # 待機組
|
|
|
|
|
self.enemy_id = None
|
|
|
|
|
|
|
|
|
|
self._detect_since = 0.0
|
|
|
|
|
self._merged_ids = [] # PURSUE/ENCIRCLE 的 4 機（領隊優先，固定順序）
|
|
|
|
|
self._merged_ex = None # 合併 executor（hover-hold）
|
|
|
|
|
self._last_replan_at = 0.0
|
|
|
|
|
self._last_plan_enemy = None # 上次重算用的敵機 (lat, lon)
|
|
|
|
|
self._last_heading = (0.0, 1.0) # 上次隊形朝向（敵機太近時沿用，避免抖動）
|
|
|
|
|
self._enemy_hist = [] # [(t, lat, lon)] 敵機軌跡（估速用）
|
|
|
|
|
self._enemy_vel = (0.0, 0.0) # 估得的敵速 (east, north) m/s
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 設定（可連續呼叫）
|
|
|
|
|
def configure(self, a_drone_ids, b_drone_ids, enemy_id):
|
|
|
|
|
"""設定 A/B 組與敵機。監看/待機階段可隨時更新；進入 PURSUE 後鎖定不受影響。"""
|
|
|
|
|
if self.phase in (MissionPhase.PURSUE, MissionPhase.ENCIRCLE):
|
|
|
|
|
return False
|
|
|
|
|
self.a_drone_ids = list(a_drone_ids)
|
|
|
|
|
self.b_drone_ids = list(b_drone_ids)
|
|
|
|
|
new_enemy = enemy_id or None
|
|
|
|
|
if new_enemy != self.enemy_id:
|
|
|
|
|
self._detect_since = 0.0 # 換敵機重置 dwell
|
|
|
|
|
self.enemy_id = new_enemy
|
|
|
|
|
return True
|
|
|
|
|
|
|
|
|
|
def _configured(self):
|
|
|
|
|
return bool(self.enemy_id) and bool(self.a_drone_ids) \
|
|
|
|
|
and self.enemy_id not in set(self.a_drone_ids) | set(self.b_drone_ids) \
|
|
|
|
|
and not (set(self.a_drone_ids) & set(self.b_drone_ids))
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 重編隊 / 停止
|
|
|
|
|
def confirm_detection(self):
|
|
|
|
|
"""DETECTED → PURSUE：操作員手動確認。停掉群組掃描，接管 4 機瞬時追蹤。"""
|
|
|
|
|
if self.phase != MissionPhase.DETECTED:
|
|
|
|
|
self.status_message.emit("目前不在待確認狀態")
|
|
|
|
|
return
|
|
|
|
|
gps = self._gps_provider()
|
|
|
|
|
epos = gps.get(self.enemy_id)
|
|
|
|
|
if not epos:
|
|
|
|
|
self.status_message.emit("敵機定位遺失，無法重編隊")
|
|
|
|
|
return
|
|
|
|
|
merged = self._merged_ids_leader_first(gps, epos)
|
|
|
|
|
if self._positions_for(merged, gps) is None:
|
|
|
|
|
self.status_message.emit("有無人機尚未定位，無法重編隊")
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# 先停掉操作員的 A/B 群組任務，避免與合併 executor 雙重送點塞爆 adapter
|
|
|
|
|
if self._stop_groups_fn:
|
|
|
|
|
try:
|
|
|
|
|
self._stop_groups_fn()
|
|
|
|
|
except Exception as e:
|
|
|
|
|
_log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")
|
|
|
|
|
|
|
|
|
|
self._merged_ids = merged
|
|
|
|
|
self._last_heading = (0.0, 1.0)
|
|
|
|
|
self._enemy_hist = []
|
|
|
|
|
self._enemy_vel = (0.0, 0.0)
|
|
|
|
|
self._update_enemy_velocity(epos)
|
|
|
|
|
slots = self._pursue_slots(gps, epos)
|
|
|
|
|
self._merged_ex = self._make_executor()
|
|
|
|
|
self._merged_ex.start(self._slots_to_plan(slots))
|
|
|
|
|
self._mark_replanned(epos)
|
|
|
|
|
self._set_phase(MissionPhase.PURSUE)
|
|
|
|
|
self.status_message.emit("重編隊確認：4 機前進中（B 追上會合）")
|
|
|
|
|
|
|
|
|
|
def stop(self):
|
|
|
|
|
"""全體停止，回 IDLE。"""
|
|
|
|
|
if self._merged_ex is not None:
|
|
|
|
|
try:
|
|
|
|
|
self._merged_ex.stop()
|
|
|
|
|
except Exception:
|
|
|
|
|
pass
|
|
|
|
|
self._merged_ex = None
|
|
|
|
|
self._merged_ids = []
|
|
|
|
|
self._detect_since = 0.0
|
|
|
|
|
self._enemy_hist = []
|
|
|
|
|
self._enemy_vel = (0.0, 0.0)
|
|
|
|
|
self._set_phase(MissionPhase.IDLE)
|
|
|
|
|
self.status_message.emit("任務已停止")
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- tick
|
|
|
|
|
def tick(self):
|
|
|
|
|
"""GUI 週期呼叫。"""
|
|
|
|
|
# 自動進出監看（偵察飛行由操作員群組任務負責，這裡只監看偵測）
|
|
|
|
|
if self.phase == MissionPhase.IDLE and self._configured():
|
|
|
|
|
self._set_phase(MissionPhase.WATCH)
|
|
|
|
|
elif self.phase == MissionPhase.WATCH and not self._configured():
|
|
|
|
|
self._detect_since = 0.0
|
|
|
|
|
self._set_phase(MissionPhase.IDLE)
|
|
|
|
|
|
|
|
|
|
if self.phase == MissionPhase.WATCH:
|
|
|
|
|
self._tick_watch()
|
|
|
|
|
elif self.phase == MissionPhase.PURSUE:
|
|
|
|
|
self._tick_pursue()
|
|
|
|
|
elif self.phase == MissionPhase.ENCIRCLE:
|
|
|
|
|
self._tick_encircle()
|
|
|
|
|
|
|
|
|
|
def _tick_watch(self):
|
|
|
|
|
gps = self._gps_provider()
|
|
|
|
|
epos = gps.get(self.enemy_id)
|
|
|
|
|
if not epos:
|
|
|
|
|
self._detect_since = 0.0
|
|
|
|
|
return
|
|
|
|
|
nearest_id, nearest_d = None, None
|
|
|
|
|
for did in self.a_drone_ids:
|
|
|
|
|
pos = gps.get(did)
|
|
|
|
|
if not pos:
|
|
|
|
|
continue
|
|
|
|
|
d = _haversine(epos['lat'], epos['lon'], pos['lat'], pos['lon'])
|
|
|
|
|
if nearest_d is None or d < nearest_d:
|
|
|
|
|
nearest_id, nearest_d = did, d
|
|
|
|
|
|
|
|
|
|
now = time.monotonic()
|
|
|
|
|
if nearest_d is not None and nearest_d <= self.r_detect:
|
|
|
|
|
if self._detect_since == 0.0:
|
|
|
|
|
self._detect_since = now
|
|
|
|
|
elif now - self._detect_since >= self.detect_dwell_sec:
|
|
|
|
|
self._set_phase(MissionPhase.DETECTED) # latch，等操作員手動確認
|
|
|
|
|
self.detection_prompt.emit(
|
|
|
|
|
f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認重編隊")
|
|
|
|
|
else:
|
|
|
|
|
self._detect_since = 0.0
|
|
|
|
|
|
|
|
|
|
def _tick_pursue(self):
|
|
|
|
|
gps = self._gps_provider()
|
|
|
|
|
epos = gps.get(self.enemy_id)
|
|
|
|
|
if not epos:
|
|
|
|
|
return # 敵機定位遺失（S4 加 stale-timeout hold）
|
|
|
|
|
self._update_enemy_velocity(epos)
|
|
|
|
|
d = self._centroid_dist(gps, epos)
|
|
|
|
|
if d is None:
|
|
|
|
|
return
|
|
|
|
|
if d <= self.r_engage:
|
|
|
|
|
self._apply_slots(self._circle_slots(gps, epos), epos)
|
|
|
|
|
self._set_phase(MissionPhase.ENCIRCLE)
|
|
|
|
|
self.status_message.emit("進入包圍")
|
|
|
|
|
return
|
|
|
|
|
if self._should_replan(time.monotonic(), epos):
|
|
|
|
|
self._apply_slots(self._pursue_slots(gps, epos), epos)
|
|
|
|
|
|
|
|
|
|
def _tick_encircle(self):
|
|
|
|
|
gps = self._gps_provider()
|
|
|
|
|
epos = gps.get(self.enemy_id)
|
|
|
|
|
if not epos:
|
|
|
|
|
return
|
|
|
|
|
self._update_enemy_velocity(epos)
|
|
|
|
|
d = self._centroid_dist(gps, epos)
|
|
|
|
|
if d is None:
|
|
|
|
|
return
|
|
|
|
|
if d > self.r_engage_exit:
|
|
|
|
|
self._apply_slots(self._pursue_slots(gps, epos), epos)
|
|
|
|
|
self._set_phase(MissionPhase.PURSUE)
|
|
|
|
|
self.status_message.emit("敵機逃離，退回前進")
|
|
|
|
|
return
|
|
|
|
|
if self._should_replan(time.monotonic(), epos):
|
|
|
|
|
self._apply_slots(self._circle_slots(gps, epos), epos)
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 高度分層
|
|
|
|
|
def _alt_for(self, did):
|
|
|
|
|
"""S3 高度分層：A 組（含領隊）走上層、B 組走下層（不低於地面下限）。
|
|
|
|
|
同高度繞圈/追擊會交叉撞機，分層後 A/B 垂直錯開自然避開。"""
|
|
|
|
|
if did in set(self.b_drone_ids):
|
|
|
|
|
return max(self.min_alt_floor,
|
|
|
|
|
self.formation_alt - self.alt_layer_gap / 2.0)
|
|
|
|
|
return self.formation_alt + self.alt_layer_gap / 2.0
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 前置攔截（估敵速）
|
|
|
|
|
def _update_enemy_velocity(self, epos):
|
|
|
|
|
"""用敵機 GPS 差分估水平速度 (east, north) m/s，視窗 enemy_vel_window_sec。"""
|
|
|
|
|
now = time.monotonic()
|
|
|
|
|
self._enemy_hist.append((now, epos['lat'], epos['lon']))
|
|
|
|
|
while (len(self._enemy_hist) >= 2
|
|
|
|
|
and now - self._enemy_hist[0][0] > self.enemy_vel_window_sec):
|
|
|
|
|
self._enemy_hist.pop(0)
|
|
|
|
|
if len(self._enemy_hist) >= 2:
|
|
|
|
|
t0, la0, lo0 = self._enemy_hist[0]
|
|
|
|
|
t1, la1, lo1 = self._enemy_hist[-1]
|
|
|
|
|
dt = t1 - t0
|
|
|
|
|
if dt > 1e-3:
|
|
|
|
|
dx, dy = _ll_to_m(la1, lo1, la0, lo0) # sample0→sample1 位移 (m)
|
|
|
|
|
self._enemy_vel = (dx / dt, dy / dt)
|
|
|
|
|
|
|
|
|
|
def enemy_speed(self):
|
|
|
|
|
"""估得的敵機水平速率 (m/s)。"""
|
|
|
|
|
return math.hypot(*self._enemy_vel)
|
|
|
|
|
|
|
|
|
|
def _aim_point(self, gps, epos):
|
|
|
|
|
"""前置攔截點：敵位 + 敵速×lead_time。lead_time = gap/friendly_speed（封頂
|
|
|
|
|
lead_time_cap_sec）。敵速尚未估到時退化為瞄當下位置（等同原純追尾）。"""
|
|
|
|
|
evx, evy = self._enemy_vel
|
|
|
|
|
lp = gps.get(self._merged_ids[0]) if self._merged_ids else None
|
|
|
|
|
gap = (_haversine(lp['lat'], lp['lon'], epos['lat'], epos['lon'])
|
|
|
|
|
if lp else 0.0)
|
|
|
|
|
lead_t = min(self.lead_time_cap_sec, gap / max(self.friendly_speed, 0.1))
|
|
|
|
|
return _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon'])
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 瞬時隊形
|
|
|
|
|
def _pursue_slots(self, gps, epos):
|
|
|
|
|
"""朝『前置攔截點』的瞬時 leader-follow：領隊朝攔截點，僚機沿隊軸後方 rank×間距、左右錯開。"""
|
|
|
|
|
ids = self._merged_ids
|
|
|
|
|
tlat, tlon = self._aim_point(gps, epos) # 前置攔截點取代敵機當下位置
|
|
|
|
|
lp = gps.get(ids[0])
|
|
|
|
|
if lp is None:
|
|
|
|
|
return None
|
|
|
|
|
lx, ly = _ll_to_m(lp['lat'], lp['lon'], tlat, tlon) # 領隊相對攔截點
|
|
|
|
|
hx, hy = -lx, -ly # 領隊 → 攔截點
|
|
|
|
|
n = math.hypot(hx, hy)
|
|
|
|
|
if n < 1.0:
|
|
|
|
|
h = self._last_heading # 太近，沿用上次朝向避免抖動
|
|
|
|
|
else:
|
|
|
|
|
h = (hx / n, hy / n)
|
|
|
|
|
self._last_heading = h
|
|
|
|
|
px, py = -h[1], h[0] # 左垂直
|
|
|
|
|
slots = {}
|
|
|
|
|
for rank, d in enumerate(ids):
|
|
|
|
|
lat_sign = -1 if rank % 2 == 0 else 1
|
|
|
|
|
# 領隊(rank0)目標≈攔截點；僚機沿 -h 後退 rank×間距，並左右錯開
|
|
|
|
|
sx = -h[0] * (rank * self.lon_spacing) + px * (lat_sign * self.lat_offset)
|
|
|
|
|
sy = -h[1] * (rank * self.lon_spacing) + py * (lat_sign * self.lat_offset)
|
|
|
|
|
lat, lon = _m_to_ll(sx, sy, tlat, tlon)
|
|
|
|
|
slots[d] = (lat, lon, self._alt_for(d))
|
|
|
|
|
return slots
|
|
|
|
|
|
|
|
|
|
def _circle_slots(self, gps, epos):
|
|
|
|
|
"""繞敵機的瞬時包圍：N 機均分在半徑 circle_radius 的圓周上。"""
|
|
|
|
|
ids = self._merged_ids
|
|
|
|
|
elat, elon = epos['lat'], epos['lon']
|
|
|
|
|
n = len(ids)
|
|
|
|
|
slots = {}
|
|
|
|
|
for i, d in enumerate(ids):
|
|
|
|
|
theta = 2.0 * math.pi * i / max(1, n)
|
|
|
|
|
x = self.circle_radius * math.cos(theta)
|
|
|
|
|
y = self.circle_radius * math.sin(theta)
|
|
|
|
|
lat, lon = _m_to_ll(x, y, elat, elon)
|
|
|
|
|
slots[d] = (lat, lon, self._alt_for(d))
|
|
|
|
|
return slots
|
|
|
|
|
|
|
|
|
|
def _apply_slots(self, slots, epos):
|
|
|
|
|
"""把瞬時 slot 熱替換給合併 executor 並更新重算基準。"""
|
|
|
|
|
if slots is None or self._merged_ex is None:
|
|
|
|
|
return
|
|
|
|
|
self._merged_ex.update_plan(self._slots_to_plan(slots))
|
|
|
|
|
self._mark_replanned(epos)
|
|
|
|
|
|
|
|
|
|
def _slots_to_plan(self, slots):
|
|
|
|
|
return {
|
|
|
|
|
'drone_ids': list(self._merged_ids),
|
|
|
|
|
'waypoints': [[slots[d]] for d in self._merged_ids],
|
|
|
|
|
'rendezvous_indices': [],
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------- 內部
|
|
|
|
|
def _centroid_dist(self, gps, epos):
|
|
|
|
|
m_pos = self._positions_for(self._merged_ids, gps)
|
|
|
|
|
if m_pos is None:
|
|
|
|
|
return None
|
|
|
|
|
cen_lat = sum(p[0] for p in m_pos) / len(m_pos)
|
|
|
|
|
cen_lon = sum(p[1] for p in m_pos) / len(m_pos)
|
|
|
|
|
return _haversine(cen_lat, cen_lon, epos['lat'], epos['lon'])
|
|
|
|
|
|
|
|
|
|
def _positions_for(self, drone_ids, gps):
|
|
|
|
|
out = []
|
|
|
|
|
for did in drone_ids:
|
|
|
|
|
p = gps.get(did)
|
|
|
|
|
if not p:
|
|
|
|
|
return None
|
|
|
|
|
out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
|
|
|
|
|
return out
|
|
|
|
|
|
|
|
|
|
def _merged_ids_leader_first(self, gps, epos):
|
|
|
|
|
"""A+B 合併；A 中離敵最近者當領隊(rank0)，其餘 A 維持原序，再接 B。"""
|
|
|
|
|
def dist(did):
|
|
|
|
|
p = gps.get(did)
|
|
|
|
|
return float('inf') if not p else _haversine(
|
|
|
|
|
epos['lat'], epos['lon'], p['lat'], p['lon'])
|
|
|
|
|
if not self.a_drone_ids:
|
|
|
|
|
return list(self.b_drone_ids)
|
|
|
|
|
leader = min(self.a_drone_ids, key=dist)
|
|
|
|
|
rest_a = [d for d in self.a_drone_ids if d != leader]
|
|
|
|
|
return [leader] + rest_a + list(self.b_drone_ids)
|
|
|
|
|
|
|
|
|
|
def _mark_replanned(self, epos):
|
|
|
|
|
self._last_replan_at = time.monotonic()
|
|
|
|
|
self._last_plan_enemy = (epos['lat'], epos['lon'])
|
|
|
|
|
|
|
|
|
|
def _should_replan(self, now, epos):
|
|
|
|
|
if self._last_plan_enemy is None:
|
|
|
|
|
return True
|
|
|
|
|
moved = _haversine(self._last_plan_enemy[0], self._last_plan_enemy[1],
|
|
|
|
|
epos['lat'], epos['lon'])
|
|
|
|
|
return (moved >= self.replan_move_m
|
|
|
|
|
or (now - self._last_replan_at) >= self.replan_cap_sec)
|
|
|
|
|
|
|
|
|
|
def _set_phase(self, phase):
|
|
|
|
|
if phase == self.phase:
|
|
|
|
|
return
|
|
|
|
|
self.phase = phase
|
|
|
|
|
_log("INFO", f"[orchestrator] phase → {phase.value}")
|
|
|
|
|
self.phase_changed.emit(phase.value)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class EngagementPanel(QWidget):
|
|
|
|
|
"""
|
|
|
|
|
交戰控制面板。
|
|
|
|
|
|
|
|
|
|
- 敵機下拉 + 即時「敵機↔最近友機」距離
|
|
|
|
|
- A/B 組指派下拉
|
|
|
|
|
- 階段顯示、重編隊 / 停止 按鈕
|
|
|
|
|
|
|
|
|
|
偵察飛行用既有的群組執行按鈕控制，這裡不含「開始」。面板只發訊號，邏輯在 GUI/orchestrator。
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
enemy_changed = pyqtSignal(str) # drone_id（未選擇=空字串）
|
|
|
|
|
confirm_requested = pyqtSignal() # 重編隊
|
|
|
|
|
stop_requested = pyqtSignal()
|
|
|
|
|
groups_changed = pyqtSignal(str, str) # (a_group_id, b_group_id)
|
|
|
|
|
apply_speed_requested = pyqtSignal(float, float) # (friendly_speed, enemy_ratio)
|
|
|
|
|
|
|
|
|
|
def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT,
|
|
|
|
|
friendly_speed=DEFAULT_FRIENDLY_SPEED):
|
|
|
|
|
super().__init__(parent)
|
|
|
|
|
self.r_detect = float(r_detect)
|
|
|
|
|
self._init_friendly_speed = float(friendly_speed)
|
|
|
|
|
self._enemy_id = ""
|
|
|
|
|
self._suppress_enemy = False
|
|
|
|
|
self._suppress_group = False
|
|
|
|
|
self._group_sig = None # 目前下拉內容的簽章，變了才重建（避免 5Hz 狂刷）
|
|
|
|
|
self._build_ui()
|
|
|
|
|
|
|
|
|
|
# ------------------------------------------------------------------ UI
|
|
|
|
|
def _build_ui(self):
|
|
|
|
|
root = QVBoxLayout(self)
|
|
|
|
|
root.setContentsMargins(12, 12, 12, 12)
|
|
|
|
|
root.setSpacing(10)
|
|
|
|
|
root.setAlignment(Qt.AlignmentFlag.AlignTop)
|
|
|
|
|
|
|
|
|
|
title = QLabel("交戰設定")
|
|
|
|
|
title.setStyleSheet(
|
|
|
|
|
"color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
|
|
|
|
|
root.addWidget(title)
|
|
|
|
|
|
|
|
|
|
combo_css = (
|
|
|
|
|
"QComboBox { background-color: #333; color: #EEE; border: 1px solid #555;"
|
|
|
|
|
" border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }")
|
|
|
|
|
|
|
|
|
|
enemy_row = QHBoxLayout()
|
|
|
|
|
enemy_row.setSpacing(8)
|
|
|
|
|
el = QLabel("敵機 sysid：")
|
|
|
|
|
el.setStyleSheet("color: #CCC; font-size: 12px;")
|
|
|
|
|
self.enemy_combo = QComboBox()
|
|
|
|
|
self.enemy_combo.setStyleSheet(combo_css)
|
|
|
|
|
self.enemy_combo.addItem(_NONE_LABEL)
|
|
|
|
|
self.enemy_combo.currentTextChanged.connect(self._on_enemy_changed)
|
|
|
|
|
enemy_row.addWidget(el)
|
|
|
|
|
enemy_row.addWidget(self.enemy_combo)
|
|
|
|
|
enemy_row.addStretch()
|
|
|
|
|
root.addLayout(enemy_row)
|
|
|
|
|
|
|
|
|
|
a_row = QHBoxLayout()
|
|
|
|
|
a_row.setSpacing(8)
|
|
|
|
|
al = QLabel("偵察組 A：")
|
|
|
|
|
al.setStyleSheet("color: #CCC; font-size: 12px;")
|
|
|
|
|
self.a_group_combo = QComboBox()
|
|
|
|
|
self.a_group_combo.setStyleSheet(combo_css)
|
|
|
|
|
self.a_group_combo.addItem(_NONE_LABEL, None)
|
|
|
|
|
self.a_group_combo.currentTextChanged.connect(self._on_group_changed)
|
|
|
|
|
a_row.addWidget(al)
|
|
|
|
|
a_row.addWidget(self.a_group_combo)
|
|
|
|
|
a_row.addStretch()
|
|
|
|
|
root.addLayout(a_row)
|
|
|
|
|
|
|
|
|
|
b_row = QHBoxLayout()
|
|
|
|
|
b_row.setSpacing(8)
|
|
|
|
|
bl = QLabel("待機組 B：")
|
|
|
|
|
bl.setStyleSheet("color: #CCC; font-size: 12px;")
|
|
|
|
|
self.b_group_combo = QComboBox()
|
|
|
|
|
self.b_group_combo.setStyleSheet(combo_css)
|
|
|
|
|
self.b_group_combo.addItem(_NONE_LABEL, None)
|
|
|
|
|
self.b_group_combo.currentTextChanged.connect(self._on_group_changed)
|
|
|
|
|
b_row.addWidget(bl)
|
|
|
|
|
b_row.addWidget(self.b_group_combo)
|
|
|
|
|
b_row.addStretch()
|
|
|
|
|
root.addLayout(b_row)
|
|
|
|
|
|
|
|
|
|
# 速度比例限速：友方 V、敵方 r×V，按鈕一次套用（DO_CHANGE_SPEED）
|
|
|
|
|
spin_css = (
|
|
|
|
|
"QDoubleSpinBox { background-color: #333; color: #EEE;"
|
|
|
|
|
" border: 1px solid #555; border-radius: 3px; padding: 2px 4px;"
|
|
|
|
|
" font-size: 12px; min-width: 70px; }")
|
|
|
|
|
speed_row = QHBoxLayout()
|
|
|
|
|
speed_row.setSpacing(8)
|
|
|
|
|
sl = QLabel("友機速度：")
|
|
|
|
|
sl.setStyleSheet("color: #CCC; font-size: 12px;")
|
|
|
|
|
self.friendly_speed_spin = QDoubleSpinBox()
|
|
|
|
|
self.friendly_speed_spin.setStyleSheet(spin_css)
|
|
|
|
|
self.friendly_speed_spin.setRange(0.5, 20.0)
|
|
|
|
|
self.friendly_speed_spin.setSingleStep(0.5)
|
|
|
|
|
self.friendly_speed_spin.setDecimals(1)
|
|
|
|
|
self.friendly_speed_spin.setSuffix(" m/s")
|
|
|
|
|
self.friendly_speed_spin.setValue(self._init_friendly_speed)
|
|
|
|
|
rl = QLabel("敵/友比例：")
|
|
|
|
|
rl.setStyleSheet("color: #CCC; font-size: 12px;")
|
|
|
|
|
self.enemy_ratio_spin = QDoubleSpinBox()
|
|
|
|
|
self.enemy_ratio_spin.setStyleSheet(spin_css)
|
|
|
|
|
self.enemy_ratio_spin.setRange(0.1, 1.0)
|
|
|
|
|
self.enemy_ratio_spin.setSingleStep(0.05)
|
|
|
|
|
self.enemy_ratio_spin.setDecimals(2)
|
|
|
|
|
self.enemy_ratio_spin.setValue(0.6)
|
|
|
|
|
speed_row.addWidget(sl)
|
|
|
|
|
speed_row.addWidget(self.friendly_speed_spin)
|
|
|
|
|
speed_row.addWidget(rl)
|
|
|
|
|
speed_row.addWidget(self.enemy_ratio_spin)
|
|
|
|
|
speed_row.addStretch()
|
|
|
|
|
root.addLayout(speed_row)
|
|
|
|
|
|
|
|
|
|
apply_speed_row = QHBoxLayout()
|
|
|
|
|
self.apply_speed_btn = QPushButton("套用速度")
|
|
|
|
|
self.apply_speed_btn.setStyleSheet(
|
|
|
|
|
"QPushButton { background-color: #00796B; color: white; border: none;"
|
|
|
|
|
" padding: 6px 14px; border-radius: 4px; font-weight: bold; }"
|
|
|
|
|
" QPushButton:hover { background-color: #00897B; }")
|
|
|
|
|
self.apply_speed_btn.clicked.connect(self._on_apply_speed)
|
|
|
|
|
self.enemy_speed_hint = QLabel("")
|
|
|
|
|
self.enemy_speed_hint.setStyleSheet("color: #777; font-size: 11px;")
|
|
|
|
|
self._refresh_enemy_speed_hint()
|
|
|
|
|
self.friendly_speed_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
|
|
|
|
|
self.enemy_ratio_spin.valueChanged.connect(self._refresh_enemy_speed_hint)
|
|
|
|
|
apply_speed_row.addWidget(self.apply_speed_btn)
|
|
|
|
|
apply_speed_row.addWidget(self.enemy_speed_hint)
|
|
|
|
|
apply_speed_row.addStretch()
|
|
|
|
|
root.addLayout(apply_speed_row)
|
|
|
|
|
|
|
|
|
|
sep = QFrame()
|
|
|
|
|
sep.setFrameShape(QFrame.Shape.HLine)
|
|
|
|
|
sep.setStyleSheet("color: #444;")
|
|
|
|
|
root.addWidget(sep)
|
|
|
|
|
|
|
|
|
|
self.distance_label = QLabel("敵機 ↔ 最近友機：—")
|
|
|
|
|
self.distance_label.setStyleSheet(
|
|
|
|
|
"color: #AAA; font-size: 13px; padding: 4px 2px;")
|
|
|
|
|
root.addWidget(self.distance_label)
|
|
|
|
|
|
|
|
|
|
hint = QLabel(f"（偵測範圍 R_detect = {self.r_detect:.0f} m）")
|
|
|
|
|
hint.setStyleSheet("color: #777; font-size: 11px;")
|
|
|
|
|
root.addWidget(hint)
|
|
|
|
|
|
|
|
|
|
self.phase_label = QLabel("階段：待機")
|
|
|
|
|
self.phase_label.setStyleSheet(
|
|
|
|
|
"color: #64B5F6; font-size: 13px; font-weight: bold; padding: 4px 2px;")
|
|
|
|
|
root.addWidget(self.phase_label)
|
|
|
|
|
|
|
|
|
|
btn_css = ("QPushButton {{ background-color: {bg}; color: {fg}; border: none;"
|
|
|
|
|
" padding: 8px 16px; border-radius: 4px; font-weight: bold; }}"
|
|
|
|
|
" QPushButton:hover {{ background-color: {hover}; }}"
|
|
|
|
|
" QPushButton:disabled {{ background-color: #444; color: #888; }}")
|
|
|
|
|
btn_row = QHBoxLayout()
|
|
|
|
|
btn_row.setSpacing(8)
|
|
|
|
|
self.confirm_btn = QPushButton("重編隊")
|
|
|
|
|
self.confirm_btn.setStyleSheet(btn_css.format(bg='#F57C00', fg='white', hover='#FB8C00'))
|
|
|
|
|
self.confirm_btn.clicked.connect(lambda: self.confirm_requested.emit())
|
|
|
|
|
self.confirm_btn.setEnabled(False)
|
|
|
|
|
self.stop_btn = QPushButton("停止")
|
|
|
|
|
self.stop_btn.setStyleSheet(btn_css.format(bg='#555', fg='#DDD', hover='#666'))
|
|
|
|
|
self.stop_btn.clicked.connect(lambda: self.stop_requested.emit())
|
|
|
|
|
btn_row.addWidget(self.confirm_btn)
|
|
|
|
|
btn_row.addWidget(self.stop_btn)
|
|
|
|
|
btn_row.addStretch()
|
|
|
|
|
root.addLayout(btn_row)
|
|
|
|
|
|
|
|
|
|
# -------------------------------------------------------------- 對外 API
|
|
|
|
|
def current_enemy(self):
|
|
|
|
|
return self._enemy_id or None
|
|
|
|
|
|
|
|
|
|
def refresh_drone_list(self, drone_ids):
|
|
|
|
|
items = [_NONE_LABEL] + sorted(drone_ids)
|
|
|
|
|
current = self._enemy_id if self._enemy_id in drone_ids else ""
|
|
|
|
|
self._suppress_enemy = True
|
|
|
|
|
self.enemy_combo.clear()
|
|
|
|
|
self.enemy_combo.addItems(items)
|
|
|
|
|
idx = self.enemy_combo.findText(current) if current else 0
|
|
|
|
|
self.enemy_combo.setCurrentIndex(idx if idx >= 0 else 0)
|
|
|
|
|
self._suppress_enemy = False
|
|
|
|
|
if current != self._enemy_id:
|
|
|
|
|
self._enemy_id = current
|
|
|
|
|
self.enemy_changed.emit(self._enemy_id)
|
|
|
|
|
|
|
|
|
|
def refresh_group_list(self, group_labels):
|
|
|
|
|
"""group_labels: list[(group_id, display_text)]。只有內容變動才重建，避免 5Hz 狂刷。"""
|
|
|
|
|
sig = tuple(group_labels)
|
|
|
|
|
if sig == self._group_sig:
|
|
|
|
|
return
|
|
|
|
|
self._group_sig = sig
|
|
|
|
|
self._suppress_group = True
|
|
|
|
|
for combo in (self.a_group_combo, self.b_group_combo):
|
|
|
|
|
prev = combo.currentData()
|
|
|
|
|
combo.clear()
|
|
|
|
|
combo.addItem(_NONE_LABEL, None)
|
|
|
|
|
for gid, text in group_labels:
|
|
|
|
|
combo.addItem(text, gid)
|
|
|
|
|
if prev is not None:
|
|
|
|
|
i = combo.findData(prev)
|
|
|
|
|
combo.setCurrentIndex(i if i >= 0 else 0)
|
|
|
|
|
self._suppress_group = False
|
|
|
|
|
|
|
|
|
|
def selected_group_ids(self):
|
|
|
|
|
return self.a_group_combo.currentData(), self.b_group_combo.currentData()
|
|
|
|
|
|
|
|
|
|
def set_phase_text(self, text):
|
|
|
|
|
self.phase_label.setText(f"階段：{text}")
|
|
|
|
|
|
|
|
|
|
def set_confirm_enabled(self, enabled):
|
|
|
|
|
self.confirm_btn.setEnabled(enabled)
|
|
|
|
|
|
|
|
|
|
def tick(self, drone_positions):
|
|
|
|
|
"""{drone_id: (lat, lon)} 更新距離讀數。"""
|
|
|
|
|
enemy = self._enemy_id
|
|
|
|
|
base = "color: #AAA; font-size: 13px; padding: 4px 2px;"
|
|
|
|
|
if not enemy:
|
|
|
|
|
self.distance_label.setText("敵機 ↔ 最近友機：（尚未選擇敵機）")
|
|
|
|
|
self.distance_label.setStyleSheet(base)
|
|
|
|
|
return
|
|
|
|
|
epos = drone_positions.get(enemy)
|
|
|
|
|
if not epos:
|
|
|
|
|
self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（等待敵機定位）")
|
|
|
|
|
self.distance_label.setStyleSheet(base)
|
|
|
|
|
return
|
|
|
|
|
nearest_id, nearest_d = None, None
|
|
|
|
|
for did, pos in drone_positions.items():
|
|
|
|
|
if did == enemy or not pos:
|
|
|
|
|
continue
|
|
|
|
|
d = _haversine(epos[0], epos[1], pos[0], pos[1])
|
|
|
|
|
if nearest_d is None or d < nearest_d:
|
|
|
|
|
nearest_id, nearest_d = did, d
|
|
|
|
|
if nearest_id is None:
|
|
|
|
|
self.distance_label.setText(f"敵機 {enemy} ↔ 最近友機：（無友機定位）")
|
|
|
|
|
self.distance_label.setStyleSheet(base)
|
|
|
|
|
return
|
|
|
|
|
color = "#E57373" if nearest_d <= self.r_detect else "#81C784"
|
|
|
|
|
self.distance_label.setText(
|
|
|
|
|
f"敵機 {enemy} ↔ 最近友機 {nearest_id}：{nearest_d:.1f} m")
|
|
|
|
|
self.distance_label.setStyleSheet(
|
|
|
|
|
f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;")
|
|
|
|
|
|
|
|
|
|
# ----------------------------------------------------------------- 內部
|
|
|
|
|
def _on_enemy_changed(self, text):
|
|
|
|
|
if self._suppress_enemy:
|
|
|
|
|
return
|
|
|
|
|
self._enemy_id = "" if text == _NONE_LABEL else text
|
|
|
|
|
_log("INFO", f"[交戰] 敵機選擇：{self._enemy_id or '（未選擇）'}")
|
|
|
|
|
self.enemy_changed.emit(self._enemy_id)
|
|
|
|
|
|
|
|
|
|
def _on_group_changed(self, _text):
|
|
|
|
|
if self._suppress_group:
|
|
|
|
|
return
|
|
|
|
|
a_id, b_id = self.selected_group_ids()
|
|
|
|
|
self.groups_changed.emit(a_id or "", b_id or "")
|
|
|
|
|
|
|
|
|
|
def _refresh_enemy_speed_hint(self, *_):
|
|
|
|
|
v = self.friendly_speed_spin.value()
|
|
|
|
|
r = self.enemy_ratio_spin.value()
|
|
|
|
|
self.enemy_speed_hint.setText(f"→ 友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s")
|
|
|
|
|
|
|
|
|
|
def _on_apply_speed(self):
|
|
|
|
|
v = self.friendly_speed_spin.value()
|
|
|
|
|
r = self.enemy_ratio_spin.value()
|
|
|
|
|
_log("INFO", f"[交戰] 套用速度：友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s (比例 {r:.2f})")
|
|
|
|
|
self.apply_speed_requested.emit(v, r)
|