From 6ef7d3a7a6d9282fa9813e5b1a961c68fc2f01c0 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 12 Aug 2026 22:57:11 +0800 Subject: [PATCH] =?UTF-8?q?feat(GUI):=20=E4=BA=A4=E6=88=B0=E4=BB=BB?= =?UTF-8?q?=E5=8B=99=E7=B7=A8=E6=8E=92=E5=99=A8=20MissionOrchestrator?= =?UTF-8?q?=EF=BC=88S0-S2=20+=20=E9=AB=98=E5=BA=A6=E5=88=86=E5=B1=A4/?= =?UTF-8?q?=E5=89=8D=E7=BD=AE=E6=94=94=E6=88=AA/=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E6=AF=94=E4=BE=8B=E9=99=90=E9=80=9F=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 在既有 MissionExecutor 之上加一層「任務大腦」，串起 ABC 對抗流程： A 組偵察 → 偵測敵機 → 手動確認重編隊 → A+B 四機前進追敵 → 足夠近展開包圍。 全部落在 src/GUI/。 新增 mission_orchestrator.py： - MissionOrchestrator 階段狀態機 IDLE→WATCH→DETECTED→PURSUE→ENCIRCLE。 - PURSUE/ENCIRCLE 用「瞬時隊形」而非路徑跟隨（每 tick 給每台目標點）， 修掉移動目標下 wp_index 歸零 + follower 外推造成的往返振盪。 - 偵測 latch（≤R_detect 持續 dwell）、moving-target 重算（位移≥2m 或封頂 1s）、 切圓 hysteresis（R_engage=15 進、R_engage_exit=20 退）。 - 高度分層 _alt_for：A 組走上層、B 組走下層（不低於地面下限）， PURSUE/ENCIRCLE 都套用 → 消掉同高度交叉撞機風險。 - 前置攔截 _aim_point：GPS 差分估敵速，瞄「敵位+敵速×lead_time」的攔截點， 治純追尾追不上；敵機靜止時退化為瞄當下。 - EngagementPanel 交戰面板：敵機下拉、A/B 組下拉、即時距離讀數、階段顯示、 重編隊/停止、以及「套用速度」（友機 V + 敵/友比例 r → DO_CHANGE_SPEED）。 mission_executor.py：加 complete_stops=False（hover-hold）+ update_plan() （熱替換航點、保留 RUNNING 閉環狀態）。 communication.py：加 set_speed()（用共用 client 泛用方法發 DO_CHANGE_SPEED(178)， 維持 GUI-only scope）。 gui.py：新增「交戰」分頁、engagement timer(5Hz)、orchestrator 接線、 _on_engagement_apply_speed（友方下 V、敵方下 r×V、同步 friendly_speed）。 SITL 實測通過：高度分層 A/B 正確分開、前置攔截追得上、套用速度後追擊/包圍穩定收斂。 Co-Authored-By: Claude Opus 4.8 --- src/GUI/communication.py | 44 ++ src/GUI/gui.py | 154 +++++++ src/GUI/mission_executor.py | 48 ++- src/GUI/mission_orchestrator.py | 705 ++++++++++++++++++++++++++++++++ 4 files changed, 950 insertions(+), 1 deletion(-) create mode 100644 src/GUI/mission_orchestrator.py diff --git a/src/GUI/communication.py b/src/GUI/communication.py index d8565bd..b6c2e88 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -1189,6 +1189,50 @@ class DroneMonitor(Node): traceback.print_exc() return False + async def set_speed(self, drone_id, speed_mps, speed_type=1): + """對指定無人機下 DO_CHANGE_SPEED(178) 設定/限制最大速度（非阻塞 async）。 + + speed_type: 0=Airspeed, 1=Ground Speed（Copter 用 1）, 2=Climb, 3=Descent。 + speed_mps: 目標速度 (m/s)。DO_CHANGE_SPEED 常數定義在 scope 外的 longCommand.py， + 故此處用共用 client 的泛用方法 _send_command_long_async 直接發 command=178， + 不修改 fc_network_apps（維持 GUI-only scope）。""" + try: + parts = drone_id.split('_') + if len(parts) < 2: + _log("ERROR", f"[SET_SPEED] 無效的 drone_id 格式: {drone_id}") + return False + sysid = int(parts[-1]) + + client = self.get_or_create_client(drone_id) + if not client: + _log("ERROR", "[SET_SPEED] CommandLongClient 無法初始化") + return False + + _log("INFO", f"[SET_SPEED] {drone_id} -> {speed_mps} m/s (type={speed_type})") + result = await client._send_command_long_async( + target_sysid=sysid, + target_compid=0, + command=178, # MAV_CMD_DO_CHANGE_SPEED + confirmation=0, + param1=float(speed_type), + param2=float(speed_mps), + param3=-1.0, # throttle 不變 + param4=0.0, # 0=absolute + param5=0.0, + param6=0.0, + param7=0.0, + timeout_sec=5.0, + ) + if result and result.success: + _log("INFO", f"[SET_SPEED] {drone_id} 速度設定成功") + return True + _log("ERROR", f"[SET_SPEED] 速度設定失敗 (message={result.message if result else 'None'})") + return False + except Exception as e: + _log("ERROR", f"[SET_SPEED] 例外錯誤: {e}") + traceback.print_exc() + return False + async def arm_drone(self, drone_id, arm): """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）""" try: diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 87092b9..17d177d 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -38,6 +38,7 @@ from mission_group import ( MissionGroup, GroupPanel, DroneAssignDialog, GROUP_COLORS, DEFAULT_MISSION_PARAM_VALUES ) +from mission_orchestrator import EngagementPanel, MissionOrchestrator, MissionPhase # ================================================================================ @@ -259,6 +260,11 @@ class ControlStationUI(QMainWindow): self.overview_timer.timeout.connect(self._flush_overview_table) self.overview_timer.start(200) + # 交戰面板：敵機↔最近友機距離讀數（5Hz） + self.engagement_timer = QTimer() + self.engagement_timer.timeout.connect(self._update_engagement_panel) + self.engagement_timer.start(200) + # 初始化連接列表 self.udp_receivers = [] self.udp_connections = [] @@ -351,6 +357,16 @@ class ControlStationUI(QMainWindow): self.settings_tab = self._create_settings_tab() self.left_tab.addTab(self.settings_tab, "設定") + # — 分頁 6：交戰（敵機選擇 + 即時距離 + 階段狀態機） + self.engagement_panel = EngagementPanel() + self.engagement_panel.enemy_changed.connect(self._on_enemy_changed) + self.engagement_panel.confirm_requested.connect(self._on_engagement_confirm) + self.engagement_panel.stop_requested.connect(self._on_engagement_stop) + self.engagement_panel.groups_changed.connect(self._on_engagement_groups_changed) + self.engagement_panel.apply_speed_requested.connect(self._on_engagement_apply_speed) + self.left_tab.addTab(self.engagement_panel, "交戰") + self._init_orchestrator() + # 右侧容器 right_container = QWidget() right_layout = QVBoxLayout(right_container) @@ -2468,6 +2484,141 @@ class ControlStationUI(QMainWindow): match = re.match(r's(\d+)_(\d+)', drone_id) return match.group(1) if match else 'unknown' + # ================================================================ + # 交戰任務編排器（MissionOrchestrator）接線 + # ================================================================ + _PHASE_LABELS = { + MissionPhase.IDLE.value: "待機（尚未設定敵機/A 組）", + MissionPhase.WATCH.value: "監看偵測中", + MissionPhase.DETECTED.value: "已偵測到敵機 — 等待確認重編隊", + MissionPhase.PURSUE.value: "前進中（4 機 leader-follow）", + MissionPhase.ENCIRCLE.value: "包圍中", + } + + def _init_orchestrator(self): + """建立 MissionOrchestrator，用 callback 與 GUI 解耦。""" + self.orchestrator = MissionOrchestrator( + make_executor=self._make_orchestrator_executor, + gps_provider=lambda: self.monitor.drone_gps, + stop_groups_fn=self._stop_engagement_group_missions, + ) + self._engagement_a_group = None + self._engagement_b_group = None + self.orchestrator.phase_changed.connect(self._on_orchestrator_phase_changed) + self.orchestrator.detection_prompt.connect(self._on_orchestrator_detection) + self.orchestrator.status_message.connect( + lambda msg: self.statusBar().showMessage(msg, 4000)) + + def _make_orchestrator_executor(self): + """為編排器建立 MissionExecutor（hover-hold：到達不自動結束，靠瞬時 slot 持續更新）。""" + ex = MissionExecutor( + sender=self.command_sender, + drone_gps=self.monitor.drone_gps, + monitor=self.monitor, + tick_rate_hz=2.0, + complete_stops=False, + ) + ex.task_status_changed.connect(self._on_task_status_changed) + return ex + + def _engagement_group_drone_ids(self, group_id): + """回傳指定 group 已分派的無人機清單（排序，穩定 rank）。""" + group = self.mission_groups.get(group_id) + if not group: + return [] + return sorted(group.selected_drone_ids) + + def _stop_engagement_group_missions(self): + """重編隊時，停掉 A/B 群組的掃描/待機任務，避免與合併 executor 雙重送點。""" + for gid in (self._engagement_a_group, self._engagement_b_group): + if gid and gid in self.mission_groups: + self._handle_group_stop(gid) + + def _sync_orchestrator_config(self): + """把面板上的敵機 + A/B 組同步進 orchestrator（監看階段可連續更新）。""" + if not hasattr(self, 'orchestrator'): + return + a_ids = self._engagement_group_drone_ids(self._engagement_a_group) \ + if self._engagement_a_group else [] + b_ids = self._engagement_group_drone_ids(self._engagement_b_group) \ + if self._engagement_b_group else [] + enemy = self.engagement_panel.current_enemy() + self.orchestrator.configure(a_ids, b_ids, enemy) + + def _update_engagement_panel(self): + """交戰面板週期更新：刷新群組清單、距離讀數、同步設定、驅動 orchestrator tick。""" + if not hasattr(self, 'engagement_panel'): + return + labels = [(gid, g.display_name) for gid, g in self.mission_groups.items()] + self.engagement_panel.refresh_group_list(labels) # 內部只在變動時重建 + self.engagement_panel.tick(self.drone_positions) + if hasattr(self, 'orchestrator'): + self._sync_orchestrator_config() + self.orchestrator.tick() + + def _on_enemy_changed(self, enemy_id): + """敵機選擇改變的回呼。""" + self._sync_orchestrator_config() + if enemy_id: + self.statusBar().showMessage(f"已指定敵機：{enemy_id}", 3000) + else: + self.statusBar().showMessage("已清除敵機指定", 3000) + + def _on_engagement_groups_changed(self, a_id, b_id): + """A/B 組指派改變 → 同步到 orchestrator。""" + self._engagement_a_group = a_id or None + self._engagement_b_group = b_id or None + self._sync_orchestrator_config() + + def _on_engagement_apply_speed(self, friendly_speed, enemy_ratio): + """套用速度比例：友方 A+B 各機下 DO_CHANGE_SPEED = V，敵機 = ratio×V。 + 同步 orchestrator.friendly_speed，讓前置攔截 lead_time 用同一速度基準。""" + enemy_speed = friendly_speed * enemy_ratio + friendly_ids = [] + for gid in (self._engagement_a_group, self._engagement_b_group): + friendly_ids.extend(self._engagement_group_drone_ids(gid) if gid else []) + enemy_id = self.engagement_panel.current_enemy() + if not friendly_ids and not enemy_id: + self.statusBar().showMessage("尚未指派 A/B 組或敵機，無法套用速度", 3000) + return + if hasattr(self, 'orchestrator'): + self.orchestrator.friendly_speed = float(friendly_speed) + + async def do_apply(): + for did in friendly_ids: + try: + await self.monitor.set_speed(did, friendly_speed) + except Exception as e: + _log("ERROR", f"[套用速度] 友機 {did} 失敗: {e}") + if enemy_id: + try: + await self.monitor.set_speed(enemy_id, enemy_speed) + except Exception as e: + _log("ERROR", f"[套用速度] 敵機 {enemy_id} 失敗: {e}") + + asyncio.run_coroutine_threadsafe(do_apply(), asyncio.get_event_loop()) + self.statusBar().showMessage( + f"套用速度：友機 {friendly_speed:.1f} m/s ({len(friendly_ids)} 台)、" + f"敵機 {enemy_speed:.1f} m/s", 4000) + + def _on_engagement_confirm(self): + if hasattr(self, 'orchestrator'): + self.orchestrator.confirm_detection() + + def _on_engagement_stop(self): + if hasattr(self, 'orchestrator'): + self.orchestrator.stop() + + def _on_orchestrator_phase_changed(self, phase_value): + """階段變更：更新面板文字與『確認重編隊』按鈕致能。""" + self.engagement_panel.set_phase_text( + self._PHASE_LABELS.get(phase_value, phase_value)) + self.engagement_panel.set_confirm_enabled( + phase_value == MissionPhase.DETECTED.value) + + def _on_orchestrator_detection(self, message): + self.statusBar().showMessage(message, 6000) + def add_drone(self, drone_id): if drone_id in self.drones: return socket_id = self.get_socket_id(drone_id) @@ -2480,6 +2631,9 @@ class ControlStationUI(QMainWindow): self.update_overview_table() # 同步新 drone 到 UI self.refresh_selection_ui() + # 同步敵機下拉選單（保留當前選擇） + if hasattr(self, 'engagement_panel'): + self.engagement_panel.refresh_drone_list(list(self.drones.keys())) def reorganize_socket_groups(self): while self.drone_panel_layout.count(): diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py index 3ebdb2d..48c980c 100644 --- a/src/GUI/mission_executor.py +++ b/src/GUI/mission_executor.py @@ -102,9 +102,13 @@ class MissionExecutor(QObject): barrier_timeout_sec=20.0, hover_stable_sec=2.0, progress_log_interval_sec=3.0, - resend_interval_sec=2.5): + resend_interval_sec=2.5, + complete_stops=True): super().__init__() self.sender = sender + # complete_stops=False：全員到達也不停（hover-hold 持續重送），供 + # orchestrator 的瞬時隊形追蹤使用（靠 update_plan 持續餵新 slot）。 + self.complete_stops = complete_stops self.drone_gps = drone_gps self.monitor = monitor # 保留：僅供外部手動 fallback 使用，閉環下不自動切 LOITER self.arrival_radius = arrival_radius @@ -167,6 +171,46 @@ class MissionExecutor(QObject): f"{rv_info}", ) + def update_plan(self, planned_waypoints): + """ + 熱替換航點（moving-target 用）。保留 RUNNING 狀態與 timer 不中斷， + 只換各機的目標座標並重置到達/送出狀態，讓下個 tick 立即朝新目標。 + + - 未在執行中 → 等同 start() + - 沿用 start() 的 planned_waypoints 格式 + - drone 集合可變動：新機補 task、消失的機移除；FALLBACK_LOITER 的機不強制復原 + """ + if self.state != MissionState.RUNNING: + self.start(planned_waypoints) + return + + drone_ids = planned_waypoints['drone_ids'] + waypoints_list = planned_waypoints['waypoints'] + self.rendezvous_indices = set( + planned_waypoints.get('rendezvous_indices', []) or [] + ) + + for i, drone_id in enumerate(drone_ids): + wps = waypoints_list[i] + task = self.tasks.get(drone_id) + if task is None: + sysid = int(drone_id.split('_')[1]) + self.tasks[drone_id] = DroneTask(drone_id, sysid, wps) + continue + task.waypoints = wps + task.wp_index = 0 + task.done = (len(wps) == 0) + task.last_sent_at = 0.0 # 下個 tick 立即重送新目標 + task.entered_radius_at = 0.0 + if task.status != TaskStatus.FALLBACK_LOITER: + task.status = TaskStatus.NORMAL + + # 移除不在新計畫中的機 + keep = set(drone_ids) + for drone_id in list(self.tasks.keys()): + if drone_id not in keep: + del self.tasks[drone_id] + def pause(self): if self.state == MissionState.RUNNING: self._timer.stop() @@ -280,6 +324,8 @@ class MissionExecutor(QObject): ) # ---- Phase 4: 完成檢查 ---- + if not self.complete_stops: + return # hover-hold 模式：不自動結束，持續重送（Phase 3 已處理） all_done = all( t.done or t.status == TaskStatus.FALLBACK_LOITER for t in self.tasks.values() diff --git a/src/GUI/mission_orchestrator.py b/src/GUI/mission_orchestrator.py new file mode 100644 index 0000000..17e5a7d --- /dev/null +++ b/src/GUI/mission_orchestrator.py @@ -0,0 +1,705 @@ +#!/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)