From a345436008e2bfd6ca3097a14ef8cc6769e590cd Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 9 Sep 2026 13:41:14 +0800 Subject: [PATCH] =?UTF-8?q?2.9.0=20=E4=BA=A4=E6=88=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/GUI/gui.py | 104 +- src/GUI/mission_group.py | 10 +- src/GUI/mission_orchestrator.py | 1947 ++++++++++++++++++++++++++---- src/GUI/mission_orchestrator1.py | 710 +++++++++++ src/GUI/mission_planner.py | 4 +- 5 files changed, 2502 insertions(+), 273 deletions(-) create mode 100644 src/GUI/mission_orchestrator1.py diff --git a/src/GUI/gui.py b/src/GUI/gui.py index e14dea1..4f83bbd 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -147,7 +147,7 @@ class ToggleSwitch(QWidget): class ControlStationUI(QMainWindow): planning_finished = pyqtSignal(object) - VERSION = '2.8.2' + VERSION = '2.9.0' FONT_SCALE_MIN = 70 FONT_SCALE_MAX = 180 FONT_SCALE_DEFAULT = 100 @@ -246,6 +246,7 @@ class ControlStationUI(QMainWindow): self.drone_positions = {} self.drone_headings = {} + self.drone_heading_updated_at = {} # 初始化地圖 self.drone_map = DroneMap() self.drone_map.set_socket_colors(self.socket_colors) @@ -277,7 +278,7 @@ class ControlStationUI(QMainWindow): # ================================================================================ self.mission_planner = FormationPlanner( spacing=5.0, # 5 公尺間距 - base_altitude=25.0, # 基準高度 25 公尺 + base_altitude=30.0, # 基準高度 30 公尺 altitude_diff=2.0 # 高低差 2 公尺 ) # ================================================================================ @@ -297,6 +298,7 @@ class ControlStationUI(QMainWindow): self.active_group_id = None # 當前 active 的 group self._group_counter = 0 # 用來產生 group_id self._pending_box_assign = None # 框選後直接分配到的 group_id + self._auto_engagement_setup_applied = False # ================================================================================ self.global_mission_defaults = dict(DEFAULT_MISSION_PARAM_VALUES) self.font_scale = self.FONT_SCALE_DEFAULT / 100.0 @@ -1146,7 +1148,7 @@ class ControlStationUI(QMainWindow): def handle_takeoff(self, drone_id): loop = asyncio.get_event_loop() - future = self.monitor.takeoff_drone(drone_id, 25.0) + future = self.monitor.takeoff_drone(drone_id, 30.0) loop.create_task(self.handle_service_response(future, f"起飛 {drone_id}")) def handle_setpoint_selected(self): @@ -1231,7 +1233,7 @@ class ControlStationUI(QMainWindow): loop = asyncio.get_event_loop() for drone_id in selected: - future = self.monitor.takeoff_drone(drone_id, 25.0) + future = self.monitor.takeoff_drone(drone_id, 30.0) loop.create_task(self.handle_service_response(future, f"批次起飛 {drone_id}")) # ================================================================================ @@ -2313,7 +2315,7 @@ class ControlStationUI(QMainWindow): _log("INFO", f"地圖點擊: {lat:.6f}, {lon:.6f} -> Group {group.group_id} ({group.mission_type})") panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} - base_alt = params.get('base_altitude', params.get('altitude', 25.0)) + base_alt = params.get('base_altitude', params.get('altitude', 30.0)) target_gps = (lat, lon, base_alt) self.statusBar().showMessage( f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000) @@ -2365,7 +2367,7 @@ class ControlStationUI(QMainWindow): panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} - base_alt = params.get('altitude', 25.0) + base_alt = params.get('altitude', 30.0) self.statusBar().showMessage( f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000) drone_gps_positions = self._collect_drone_gps(selected_drones) @@ -2426,7 +2428,7 @@ class ControlStationUI(QMainWindow): panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} - base_alt = params.get('altitude', 25.0) + base_alt = params.get('altitude', 30.0) self.statusBar().showMessage( f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000) @@ -2632,15 +2634,43 @@ class ControlStationUI(QMainWindow): MissionPhase.IDLE.value: "待機（尚未設定敵機/A 組）", MissionPhase.WATCH.value: "監看偵測中", MissionPhase.DETECTED.value: "已偵測到敵機 — 等待確認重編隊", - MissionPhase.PURSUE.value: "前進中（4 機 leader-follow）", + MissionPhase.CONVERGE.value: "收斂中（動態分配包圍位置）", MissionPhase.ENCIRCLE.value: "包圍中", + MissionPhase.TARGET_LOST.value: "敵機定位遺失（友機保持位置）", } + def _engagement_gps_provider(self): + """ + 提供交戰編排器完整的定位快照。 + + GPS 與 VFR_HUD/heading 是不同訊息來源；monitor.drone_gps 原本只有 + lat/lon/alt，因此在這裡把 GUI 最新 heading 合併進去，避免下一筆 GPS + 覆蓋資料後讓交戰 X 陣形失去敵機機首方向。 + """ + source = getattr(self.monitor, 'drone_gps', {}) + snapshot = { + drone_id: dict(pos) + for drone_id, pos in source.items() + if isinstance(pos, dict) + } + now = time.monotonic() + for drone_id, heading in self.drone_headings.items(): + updated_at = self.drone_heading_updated_at.get(drone_id) + heading_is_fresh = ( + isinstance(updated_at, (int, float)) + and now - updated_at <= 2.0 + ) + if (drone_id in snapshot + and isinstance(heading, (int, float)) + and heading_is_fresh): + snapshot[drone_id]['heading'] = float(heading) % 360.0 + return snapshot + def _init_orchestrator(self): """建立 MissionOrchestrator，用 callback 與 GUI 解耦。""" self.orchestrator = MissionOrchestrator( make_executor=self._make_orchestrator_executor, - gps_provider=lambda: self.monitor.drone_gps, + gps_provider=self._engagement_gps_provider, stop_groups_fn=self._stop_engagement_group_missions, ) self._engagement_a_group = None @@ -2776,6 +2806,61 @@ class ControlStationUI(QMainWindow): # 同步敵機下拉選單（保留當前選擇） if hasattr(self, 'engagement_panel'): self.engagement_panel.refresh_drone_list(list(self.drones.keys())) + self._try_auto_engagement_setup() + + def _try_auto_engagement_setup(self): + """sysid 11~15 全部出現時，自動建立 ABC 編組並設定交戰面板。""" + if self._auto_engagement_setup_applied: + return + + drones_by_sysid = {} + for did in sorted(self.drones): + match = re.fullmatch(r's\d+_(\d+)', did) + if match: + drones_by_sysid.setdefault(int(match.group(1)), did) + + required_sysids = {11, 12, 13, 14, 15} + if not required_sysids.issubset(drones_by_sysid): + return + + # 正常啟動時已有 A；補建 B、C。 + while self._group_counter < 3: + self._add_mission_group() + if not {'A', 'B', 'C'}.issubset(self.mission_groups): + _log("WARN", "自動交戰編組失敗：找不到完整的 Group A/B/C") + return + + assignments = { + 'A': {drones_by_sysid[11], drones_by_sysid[12]}, + 'B': {drones_by_sysid[14], drones_by_sysid[15]}, + 'C': {drones_by_sysid[13]}, + } + for gid, members in assignments.items(): + group = self.mission_groups[gid] + if group.executor: + group.executor.stop() + group.selected_drone_ids = set(members) + group.planned_waypoints = None + group.mission_target = None + panel = self.group_panels.get(gid) + if panel: + panel.update_drone_list() + panel.update_status() + panel.clear_mission_info() + + self._auto_engagement_setup_applied = True + self.refresh_selection_ui() + labels = [(gid, group.display_name) + for gid, group in self.mission_groups.items()] + self.engagement_panel.refresh_group_list(labels) + enemy_id = drones_by_sysid[13] + if not self.engagement_panel.set_configuration(enemy_id, 'A', 'B'): + _log("WARN", "自動交戰編組完成，但交戰面板設定失敗") + return + self._sync_orchestrator_config() + self.statusBar().showMessage( + "已自動設定交戰：A=sysid 11,12；B=sysid 14,15；敵機=sysid 13（Group C）", + 6000) def reorganize_socket_groups(self): while self.drone_panel_layout.count(): @@ -2986,6 +3071,7 @@ class ControlStationUI(QMainWindow): heading = hud_data.get('heading') if isinstance(heading, (int, float)): self.drone_headings[drone_id] = heading + self.drone_heading_updated_at[drone_id] = time.monotonic() self._map_dirty_drones.add(drone_id) groundspeed = hud_data.get('groundspeed') airspeed = hud_data.get('airspeed') diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index 9ffc30e..65eb079 100644 --- a/src/GUI/mission_group.py +++ b/src/GUI/mission_group.py @@ -24,10 +24,10 @@ GROUP_COLORS = [ DEFAULT_MISSION_PARAM_VALUES = { 'spacing': '5.0', - 'base_altitude': '25.0', + 'base_altitude': '30.0', 'altitude_diff': '2.0', 'radius': '10.0', - 'altitude': '25.0', + 'altitude': '30.0', 'start_angle': '0', 'lateral_offset': '3.0', 'longitudinal_spacing': '5.0', @@ -249,8 +249,8 @@ class GroupPanel(QWidget): takeoff_row.setSpacing(3) self.alt_input = QComboBox() self.alt_input.setEditable(True) - self.alt_input.addItems(["5", "10", "15", "20", "25"]) - self.alt_input.setCurrentText("25") + self.alt_input.addItems(["5", "10", "15", "20", "25", "30"]) + self.alt_input.setCurrentText("30") self.alt_input.setStyleSheet(COMBO) alt_lbl = QLabel("m") alt_lbl.setStyleSheet(LBL) @@ -679,5 +679,5 @@ class GroupPanel(QWidget): try: alt = float(self.alt_input.currentText()) except ValueError: - alt = 25.0 + alt = 30.0 self.takeoff_requested.emit(self.group.group_id, alt) diff --git a/src/GUI/mission_orchestrator.py b/src/GUI/mission_orchestrator.py index a62fc3d..f963657 100644 --- a/src/GUI/mission_orchestrator.py +++ b/src/GUI/mission_orchestrator.py @@ -1,225 +1,545 @@ #!/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）。 +MissionOrchestrator —— 多機偵測後的動態包圍任務編排器。 + +設計重點 +======== +1. 偵察階段仍可使用既有 A/B 群組：A 搜尋、B 待機。 +2. 偵測並由操作員確認後，A/B 角色立即解除，所有友機視為同一個集合。 +3. 不再使用 leader-follow / PURSUE 隊形；確認後直接針對敵機未來位置的包圍 slot 收斂。 + A/B 高度以敵機即時相對高度為基準，不再固定以 30m 為中心。 +4. 每台友機使用 C2 平滑且持續前進的 quintic Cartesian「微幅發散 → 收斂」虛擬軌跡，避免繞大圈。 +5. 對所有 slot assignment（4 機時最多 4! = 24 種）評分： + - 路徑長度 + - 路線交叉 + - 預測最小間距 + - 頻繁交換 slot 的成本 +6. 同層路徑交叉維持硬限制；間距不足時在持續前進中疊加分離修正。 + A/B 跨層若水平距離過近且尚未建立垂直間隔，先原地垂直分層，再水平移動。 +7. 包圍完成條件改為「每台友機均到達自己的 slot」，不是形心距敵機。 +8. 敵機定位遺失超時時進 TARGET_LOST，將友機 hold 在當下位置，避免繼續追過期目標。 + +相容性 +====== +- 保留既有 MissionExecutor 介面：start(plan) / update_plan(plan) / stop() +- 保留 EngagementPanel 的主要 signal/API。 +- __init__ 保留舊版 r_engage / r_engage_exit / lon_spacing / lat_offset 參數， + 讓既有 GUI 呼叫端不必立刻全部修改；新策略不再依賴 leader-follow 的 spacing。 """ + import math import time from enum import Enum +from itertools import permutations, product from PyQt6.QtWidgets import ( - QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, QPushButton, - QDoubleSpinBox + QWidget, QVBoxLayout, QHBoxLayout, QLabel, QComboBox, QFrame, + QPushButton, QDoubleSpinBox ) from PyQt6.QtCore import Qt, QObject, pyqtSignal +# --------------------------------------------------------------------------- +# 基礎工具 + +EARTH_R = 6371000.0 + + def _log(level, message): print(f"[{level}] {message}", flush=True) def _haversine(lat1, lon1, lat2, lon2): - """兩經緯度間的水平地面距離 (m)。""" - R = 6371000.0 + """兩經緯度間水平地面距離 (m)。""" 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))) + a = (math.sin(dphi / 2.0) ** 2 + + math.cos(p1) * math.cos(p2) * math.sin(dlam / 2.0) ** 2) + return 2.0 * EARTH_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 + """經緯度 -> 以 ref 為原點的局部平面公尺座標 (x=東, y=北)。""" + x = math.radians(lon - ref_lon) * EARTH_R * math.cos(math.radians(ref_lat)) + y = math.radians(lat - ref_lat) * EARTH_R 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)))) + """局部平面公尺座標 -> 經緯度。""" + lat = ref_lat + math.degrees(y / EARTH_R) + cos_lat = max(1e-9, math.cos(math.radians(ref_lat))) + lon = ref_lon + math.degrees(x / (EARTH_R * cos_lat)) return lat, lon +def _norm2(x, y): + return math.hypot(x, y) + + +def _normalize(x, y, fallback=(1.0, 0.0)): + n = math.hypot(x, y) + if n < 1e-9: + return fallback + return x / n, y / n + + +def _bezier_point(p0, p1, p2, p3, t): + """2D cubic Bezier。pX=(x,y)。""" + u = 1.0 - t + b0 = u ** 3 + b1 = 3.0 * u * u * t + b2 = 3.0 * u * t * t + b3 = t ** 3 + return ( + b0 * p0[0] + b1 * p1[0] + b2 * p2[0] + b3 * p3[0], + b0 * p0[1] + b1 * p1[1] + b2 * p2[1] + b3 * p3[1], + ) + + +def _smoothstep5(t): + """Quintic smoothstep: 0/1 端點的一、二階導數皆為 0。""" + t = min(1.0, max(0.0, float(t))) + return t * t * t * (t * (t * 6.0 - 15.0) + 10.0) + + +def _wrap_pi(a): + """角度包到 [-pi, pi)。""" + return (a + math.pi) % (2.0 * math.pi) - math.pi + + +def _polyline_length(points): + total = 0.0 + for a, b in zip(points, points[1:]): + total += math.hypot(b[0] - a[0], b[1] - a[1]) + return total + + +def _orientation(a, b, c): + return ((b[0] - a[0]) * (c[1] - a[1]) + - (b[1] - a[1]) * (c[0] - a[0])) + + +def _segments_intersect(a, b, c, d, eps=1e-9): + """一般位置線段相交測試；對任務規劃足夠。""" + o1 = _orientation(a, b, c) + o2 = _orientation(a, b, d) + o3 = _orientation(c, d, a) + o4 = _orientation(c, d, b) + + # 只把真正穿越當成 crossing；共線/端點接觸交給 minimum separation 處理。 + return ((o1 > eps and o2 < -eps) or (o1 < -eps and o2 > eps)) and \ + ((o3 > eps and o4 < -eps) or (o3 < -eps and o4 > eps)) + + +# --------------------------------------------------------------------------- # 預設參數 -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 = 25.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) + +DEFAULT_R_DETECT = 40.0 +DEFAULT_DETECT_DWELL = 1.0 + +# 以下兩個保留作為舊呼叫端相容參數；新策略不用「距離到某半徑才切追擊/包圍」。 +DEFAULT_R_ENGAGE = 25.0 +DEFAULT_R_ENGAGE_EXIT = 35.0 + +# 新策略建議值 +DEFAULT_CIRCLE_RADIUS = 10.0 # 最終包圍半徑 (m)；近場會以敵機當下位置為圓心 +DEFAULT_FORMATION_ALT = 30.0 # 敵機高度缺失時的安全備援值 (m，相對 home) +DEFAULT_ALT_LAYER_GAP = 10.0 # A/B 兩層的垂直間隔 (m) +DEFAULT_MIN_ALT_FLOOR = 10.0 # 最低安全相對高度 (m) +DEFAULT_CROSS_LAYER_HOLD_DISTANCE = 12.0 # A/B 水平距離小於此值時先確認垂直分層 (m) +DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP = 7.0 # 跨層近距離時要求的最小實際垂直間隔 (m) +DEFAULT_CROSS_LAYER_ALT_TOLERANCE = 1.0 # 垂直分層完成容許誤差 (m) + +DEFAULT_SPREAD_DISTANCE = 3.0 # 小幅外擴上限 (m)：避免繞大圈，只保留必要的發散 +DEFAULT_APPROACH_MARGIN = 4.0 # 保留相容參數；新 Cartesian 平滑路徑不再強制繞到圈外 staging +DEFAULT_LOOKAHEAD_T = 0.35 # 遠距離時的 Bezier 前視比例 +DEFAULT_FINAL_DIRECT_DISTANCE = 4.0 # 只在最後 4m 直接送 slot；避免中途提早進入減速 +DEFAULT_LEAD_FADE_START = 30.0 # 平均距敵 <= 此距離開始逐步減少敵機前置預測 +DEFAULT_LEAD_FADE_END = 18.0 # 平均距敵 <= 此距離完全取消前置，圓心貼住敵機當下位置 +DEFAULT_CENTER_SMOOTH_ALPHA = 0.25 # 規劃圓心低通係數，降低 GPS/敵速造成的路徑抖動 +DEFAULT_LOOKAHEAD_DISTANCE = 14.0 # 保持較遠 carrot point，避免反覆追短 waypoint 而減速 +DEFAULT_PATH_SAMPLES = 41 # 路徑/交叉預測取樣數；提高以降低漏判 +DEFAULT_SAFE_SEPARATION = 8.0 # 硬安全間距 (m) +DEFAULT_FORMATION_SPACING = 12.0 # 移動中希望逐步拉到的隊形間距；比硬安全距離更早開始展開 +DEFAULT_INITIAL_SEP_TOLERANCE = 0.5 # 起始若小於 8m，允許最多只再縮短這麼多 (m) +DEFAULT_EMERGENCY_SEPARATION = 3.0 # 無論如何不可低於的水平緊急下限 (m) +DEFAULT_RECOVERY_SPREAD_STEP = 2.0 # 舊版保底：無 assignment 時的小幅外擴 (m) +DEFAULT_SEPARATION_CORRECTION_GAIN = 1.25 # 更積極的移動中隊形修正 +DEFAULT_SEPARATION_CORRECTION_MAX = 5.0 # 最大側向修正；仍保證主要前進分量 +DEFAULT_DIRECTION_SMOOTH_ALPHA = 0.35 # 每 tick 接受新方向的比例；越小越平滑 +DEFAULT_MAX_LATERAL_RATIO = 0.45 # 側向/前進最大比例，避免修隊形時原地橫移 +DEFAULT_LAYER_LANE_SPACING = 4.0 # 同層同心圓車道的半徑間隔 (m) +DEFAULT_LAYER_AVOID_DISTANCE = 14.0 # 同層距離小於此值開始啟用同心圓避讓 +DEFAULT_LAYER_LANE_FADE_DISTANCE = 12.0 # 距最終 slot 小於此值時逐步收回共同包圍半徑 +DEFAULT_HEADING_SMOOTH_ALPHA = 0.25 # 敵機 heading 低通，避免 X 陣形因航向抖動快速旋轉 +DEFAULT_ENCIRCLE_TOLERANCE = 3.0 # 每台距自己 slot <= 此值才算完成包圍 +DEFAULT_REASSIGN_IMPROVEMENT = 0.20 # 安全時至少改善 20% 才允許換 slot +DEFAULT_REASSIGN_COOLDOWN = 2.0 # slot 交換最短間隔 (s)；安全時原則上不換 + +DEFAULT_REPLAN_MOVE_M = 1.0 # 敵機移動 >= 1m 觸發更新 +DEFAULT_REPLAN_CAP_SEC = 0.4 # 最慢 0.4 秒更新一次瞬時目標 +DEFAULT_LEAD_TIME_CAP = 4.0 +DEFAULT_FRIENDLY_SPEED = 5.0 +DEFAULT_ENEMY_VEL_WINDOW = 1.0 +DEFAULT_TARGET_LOST_TIMEOUT = 2.0 + +# assignment cost +DEFAULT_CROSSING_PENALTY = 20000.0 +DEFAULT_SEPARATION_PENALTY = 30000.0 +DEFAULT_SLOT_SWITCH_PENALTY = 8.0 + +# 舊版相容參數：保留但不再當 leader-follow spacing 使用 +DEFAULT_LON_SPACING = 8.0 +DEFAULT_LAT_OFFSET = 5.0 _NONE_LABEL = "（未選擇）" class MissionPhase(Enum): - IDLE = "idle" # 未設定敵機/A 組 - WATCH = "watch" # 監看偵測（偵察飛行由操作員的群組任務負責） - DETECTED = "detected" # 偵測 latch，等操作員確認重編隊 - PURSUE = "pursue" # 4 機瞬時 leader-follow 朝敵機 - ENCIRCLE = "encircle" # 4 機包圍敵機 + IDLE = "idle" + WATCH = "watch" + DETECTED = "detected" + CONVERGE = "converge" # 動態分配 slot + 發散/收斂曲線接近 + ENCIRCLE = "encircle" # 所有友機已進入自己的 slot + TARGET_LOST = "target_lost" # 目標定位逾時，友機 hold + +# --------------------------------------------------------------------------- +# Mission Orchestrator class MissionOrchestrator(QObject): """ - 階段狀態機。透過注入的 callback 與 GUI 解耦（便於 headless 測試）： - make_executor() -> MissionExecutor（complete_stops=False，hover-hold） + 任務狀態機。 + + callback： + make_executor() -> MissionExecutor gps_provider() -> {drone_id: {'lat','lon','alt'}} - stop_groups_fn() -> 停掉操作員的 A/B 群組任務（重編隊時避免雙重送點） + stop_groups_fn() -> 操作員確認重編隊時，停止既有 A/B 群組任務 + + 新策略： + WATCH -> DETECTED -> CONVERGE -> ENCIRCLE + + 進入 CONVERGE 後保留 A/B 分層：A/B 各自固定在敵機高度上下層； + 同層必要時使用同心圓半徑車道避讓。跨層近距離時先垂直、後水平。 """ - phase_changed = pyqtSignal(str) # MissionPhase.value - detection_prompt = pyqtSignal(str) # 偵測提示（含最近友機/距離） + phase_changed = pyqtSignal(str) + 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): + 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, + spread_distance=DEFAULT_SPREAD_DISTANCE, + approach_margin=DEFAULT_APPROACH_MARGIN, + lookahead_t=DEFAULT_LOOKAHEAD_T, + final_direct_distance=DEFAULT_FINAL_DIRECT_DISTANCE, + lead_fade_start=DEFAULT_LEAD_FADE_START, + lead_fade_end=DEFAULT_LEAD_FADE_END, + center_smooth_alpha=DEFAULT_CENTER_SMOOTH_ALPHA, + lookahead_distance=DEFAULT_LOOKAHEAD_DISTANCE, + path_samples=DEFAULT_PATH_SAMPLES, + safe_separation=DEFAULT_SAFE_SEPARATION, + formation_spacing=DEFAULT_FORMATION_SPACING, + initial_sep_tolerance=DEFAULT_INITIAL_SEP_TOLERANCE, + emergency_separation=DEFAULT_EMERGENCY_SEPARATION, + recovery_spread_step=DEFAULT_RECOVERY_SPREAD_STEP, + separation_correction_gain=DEFAULT_SEPARATION_CORRECTION_GAIN, + separation_correction_max=DEFAULT_SEPARATION_CORRECTION_MAX, + direction_smooth_alpha=DEFAULT_DIRECTION_SMOOTH_ALPHA, + max_lateral_ratio=DEFAULT_MAX_LATERAL_RATIO, + layer_lane_spacing=DEFAULT_LAYER_LANE_SPACING, + layer_avoid_distance=DEFAULT_LAYER_AVOID_DISTANCE, + layer_lane_fade_distance=DEFAULT_LAYER_LANE_FADE_DISTANCE, + heading_smooth_alpha=DEFAULT_HEADING_SMOOTH_ALPHA, + encircle_tolerance=DEFAULT_ENCIRCLE_TOLERANCE, + reassign_improvement=DEFAULT_REASSIGN_IMPROVEMENT, + reassign_cooldown_sec=DEFAULT_REASSIGN_COOLDOWN, + target_lost_timeout_sec=DEFAULT_TARGET_LOST_TIMEOUT, + crossing_penalty=DEFAULT_CROSSING_PENALTY, + separation_penalty=DEFAULT_SEPARATION_PENALTY, + slot_switch_penalty=DEFAULT_SLOT_SWITCH_PENALTY, + parent=None, + cross_layer_hold_distance=DEFAULT_CROSS_LAYER_HOLD_DISTANCE, + cross_layer_min_vertical_gap=DEFAULT_CROSS_LAYER_MIN_VERTICAL_GAP, + cross_layer_alt_tolerance=DEFAULT_CROSS_LAYER_ALT_TOLERANCE, + ): 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) + + # legacy/compatibility self.r_engage = float(r_engage) self.r_engage_exit = float(r_engage_exit) + self.lon_spacing = float(lon_spacing) + self.lat_offset = float(lat_offset) + 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.cross_layer_hold_distance = max( + 0.0, float(cross_layer_hold_distance) + ) + self.cross_layer_min_vertical_gap = max( + 0.0, float(cross_layer_min_vertical_gap) + ) + self.cross_layer_alt_tolerance = max( + 0.0, float(cross_layer_alt_tolerance) + ) 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.spread_distance = float(spread_distance) + self.approach_margin = float(approach_margin) + self.lookahead_t = min(0.95, max(0.05, float(lookahead_t))) + self.final_direct_distance = max(0.0, float(final_direct_distance)) + self.lead_fade_start = max(0.0, float(lead_fade_start)) + self.lead_fade_end = max(0.0, float(lead_fade_end)) + self.center_smooth_alpha = min(1.0, max(0.02, float(center_smooth_alpha))) + self.lookahead_distance = max(1.0, float(lookahead_distance)) + if self.lead_fade_end > self.lead_fade_start: + self.lead_fade_end, self.lead_fade_start = self.lead_fade_start, self.lead_fade_end + self.path_samples = max(7, int(path_samples)) + self.safe_separation = float(safe_separation) + self.formation_spacing = max(self.safe_separation, float(formation_spacing)) + self.initial_sep_tolerance = max(0.0, float(initial_sep_tolerance)) + self.emergency_separation = max(0.5, float(emergency_separation)) + self.recovery_spread_step = max(0.5, float(recovery_spread_step)) + self.separation_correction_gain = max(0.0, float(separation_correction_gain)) + self.separation_correction_max = max(0.0, float(separation_correction_max)) + self.direction_smooth_alpha = min(1.0, max(0.05, float(direction_smooth_alpha))) + self.max_lateral_ratio = min(1.0, max(0.05, float(max_lateral_ratio))) + self.layer_lane_spacing = max(0.5, float(layer_lane_spacing)) + self.layer_avoid_distance = max(self.safe_separation, float(layer_avoid_distance)) + self.layer_lane_fade_distance = max(1.0, float(layer_lane_fade_distance)) + self.heading_smooth_alpha = min(1.0, max(0.02, float(heading_smooth_alpha))) + self.encircle_tolerance = float(encircle_tolerance) + self.reassign_improvement = min(0.9, max(0.0, float(reassign_improvement))) + self.reassign_cooldown_sec = float(reassign_cooldown_sec) + self.target_lost_timeout_sec = float(target_lost_timeout_sec) + self.crossing_penalty = float(crossing_penalty) + self.separation_penalty = float(separation_penalty) + self.slot_switch_penalty = float(slot_switch_penalty) + self.phase = MissionPhase.IDLE - self.a_drone_ids = [] # 偵察組 - self.b_drone_ids = [] # 待機組 + 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._friendly_ids = [] + self._merged_ex = None + + # assignment: {drone_id: slot_index} + self._assignment = {} + self._last_assignment_change_at = 0.0 + self._last_assignment_had_conflict = False + + # 敵機運動估計 + self._enemy_hist = [] + self._enemy_vel = (0.0, 0.0) # east, north m/s + self._enemy_alt = None # 敵機即時相對高度；缺失時才用 formation_alt + self._enemy_heading_deg = None # 0°=North, clockwise；X 陣形以此為基準 + self._last_enemy_seen_at = 0.0 + + # replan / command state 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 + self._last_plan_enemy = None + self._last_predicted_center = None + self._last_slots = None + self._smoothed_center = None + # 每架持續前進方向；避免每次 replan 重新生成 carrot 時航向跳動/煞停 + self._motion_dirs = {} + self._slot_groups = {} # {slot_index: "A"/"B"} + self._vertical_hold_ids = set() + + # ------------------------------------------------------------------ 設定 - # ------------------------------------------------------------- 設定（可連續呼叫） def configure(self, a_drone_ids, b_drone_ids, enemy_id): - """設定 A/B 組與敵機。監看/待機階段可隨時更新；進入 PURSUE 後鎖定不受影響。""" - if self.phase in (MissionPhase.PURSUE, MissionPhase.ENCIRCLE): + """ + WATCH/DETECTED 階段可更新 A/B 與敵機。 + 一旦進入 CONVERGE/ENCIRCLE，角色集合鎖定，避免 GUI 下拉變動破壞任務。 + """ + if self.phase in ( + MissionPhase.CONVERGE, + MissionPhase.ENCIRCLE, + MissionPhase.TARGET_LOST, + ): 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._detect_since = 0.0 + self._enemy_hist = [] + self._enemy_vel = (0.0, 0.0) + self._enemy_alt = None + self._enemy_heading_deg = None 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) \ + friendly = set(self.a_drone_ids) | set(self.b_drone_ids) + return ( + bool(self.enemy_id) + and bool(self.a_drone_ids) + and self.enemy_id not in friendly and not (set(self.a_drone_ids) & set(self.b_drone_ids)) + ) + + # ------------------------------------------------------------------ 操作員確認 / 停止 - # ------------------------------------------------------------- 重編隊 / 停止 def confirm_detection(self): - """DETECTED → PURSUE：操作員手動確認。停掉群組掃描，接管 4 機瞬時追蹤。""" + """ + DETECTED -> CONVERGE。 + + 1. 停止既有 A/B 群組任務。 + 2. A/B 身分解除，合併為單一 friendly 集合。 + 3. 計算預測敵機中心與圓形 slots。 + 4. 做全域 slot assignment。 + 5. 產生 Bezier lookahead point，啟動合併 executor。 + """ 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("敵機定位遺失，無法重編隊") + 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("有無人機尚未定位，無法重編隊") + + friendly_ids = self._unique_keep_order(self.a_drone_ids + self.b_drone_ids) + if not friendly_ids: + self.status_message.emit("沒有可用友機") + return + if self._positions_for(friendly_ids, 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._friendly_ids = friendly_ids + self._assignment = {} + self._last_assignment_change_at = 0.0 + self._last_assignment_had_conflict = False self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) + self._enemy_alt = None + self._enemy_heading_deg = None + self._smoothed_center = None + self._motion_dirs = {} + self._slot_groups = {} # {slot_index: "A"/"B"} + self._vertical_hold_ids = set() + self._last_enemy_seen_at = time.monotonic() self._update_enemy_velocity(epos) - slots = self._pursue_slots(gps, epos) + + center = self._planning_center(gps, epos, reset=True) + slots = self._circle_slots_for_center(center, len(self._friendly_ids)) + assignment, info = self._choose_assignment(gps, center, slots, force=True) + self._merged_ex = self._make_executor() - self._merged_ex.start(self._slots_to_plan(slots)) + if not assignment: + # 找不到完全滿足間距的方案時，不再先停/預分散。 + # 改採 0-crossing 的最低風險 provisional assignment， + # 一邊朝 slot 前進、一邊疊加 separation correction 拉開隊形。 + assignment, info = self._choose_progress_assignment(gps, center, slots) + if not assignment: + self.status_message.emit("目前找不到 0-crossing 路徑，暫不開始包圍") + self._merged_ex = None + return + self._assignment = assignment + self._last_assignment_change_at = time.monotonic() + self._last_assignment_had_conflict = True + targets = self._moving_converge_targets(gps, center, slots, assignment) + if targets is None: + self.status_message.emit("無法建立移動中隊形調整目標") + self._merged_ex = None + return + self._merged_ex.start(self._targets_to_plan(targets)) + self.status_message.emit( + "起始間距不足：保持前進，同時在移動中逐步拉開隊形" + ) + else: + self._assignment = assignment + self._last_assignment_change_at = time.monotonic() + self._last_assignment_had_conflict = info['conflict'] + targets = self._moving_converge_targets(gps, center, slots, assignment) + self._merged_ex.start(self._targets_to_plan(targets)) + + self._last_predicted_center = center + self._last_slots = slots self._mark_replanned(epos) - self._set_phase(MissionPhase.PURSUE) - self.status_message.emit("重編隊確認：4 機前進中（B 追上會合）") + self._set_phase(MissionPhase.CONVERGE) + + if assignment: + self.status_message.emit("開始動態包圍：slot 鎖定，移動中同步展開隊形") def stop(self): - """全體停止，回 IDLE。""" + """停止任務並回 IDLE。""" if self._merged_ex is not None: try: self._merged_ex.stop() except Exception: pass - self._merged_ex = None - self._merged_ids = [] + self._merged_ex = None + self._friendly_ids = [] + self._assignment = {} self._detect_since = 0.0 self._enemy_hist = [] self._enemy_vel = (0.0, 0.0) + self._enemy_alt = None + self._enemy_heading_deg = None + self._last_enemy_seen_at = 0.0 + self._last_predicted_center = None + self._last_slots = None + self._smoothed_center = None + self._motion_dirs = {} + self._slot_groups = {} # {slot_index: "A"/"B"} + self._vertical_hold_ids = set() self._set_phase(MissionPhase.IDLE) self.status_message.emit("任務已停止") - # ------------------------------------------------------------- tick + # ------------------------------------------------------------------ tick + def tick(self): - """GUI 週期呼叫。""" - # 自動進出監看（偵察飛行由操作員群組任務負責，這裡只監看偵測） + """由 GUI 週期呼叫，建議 5~10 Hz。""" if self.phase == MissionPhase.IDLE and self._configured(): self._set_phase(MissionPhase.WATCH) elif self.phase == MissionPhase.WATCH and not self._configured(): @@ -228,10 +548,12 @@ class MissionOrchestrator(QObject): if self.phase == MissionPhase.WATCH: self._tick_watch() - elif self.phase == MissionPhase.PURSUE: - self._tick_pursue() + elif self.phase == MissionPhase.CONVERGE: + self._tick_converge() elif self.phase == MissionPhase.ENCIRCLE: self._tick_encircle() + elif self.phase == MissionPhase.TARGET_LOST: + self._tick_target_lost() def _tick_watch(self): gps = self._gps_provider() @@ -239,7 +561,9 @@ class MissionOrchestrator(QObject): if not epos: self._detect_since = 0.0 return - nearest_id, nearest_d = None, None + + nearest_id = None + nearest_d = None for did in self.a_drone_ids: pos = gps.get(did) if not pos: @@ -253,169 +577,1184 @@ class MissionOrchestrator(QObject): 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._set_phase(MissionPhase.DETECTED) self.detection_prompt.emit( - f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認重編隊") + f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認動態包圍" + ) else: self._detect_since = 0.0 - def _tick_pursue(self): + def _tick_converge(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) + now = time.monotonic() + if not epos: - return # 敵機定位遺失（S4 加 stale-timeout hold） + self._handle_enemy_missing(gps, now) + return + + self._last_enemy_seen_at = now self._update_enemy_velocity(epos) - d = self._centroid_dist(gps, epos) - if d is None: + + if self._positions_for(self._friendly_ids, gps) is None: return - if d <= self.r_engage: - self._apply_slots(self._circle_slots(gps, epos), epos) + + if self._all_in_slots(gps, epos): self._set_phase(MissionPhase.ENCIRCLE) - self.status_message.emit("進入包圍") + self.status_message.emit("所有友機已進入各自包圍 slot") + # 進 ENCIRCLE 當下仍送一次精確 slot + self._update_encircle_command(gps, epos, force=True) return - if self._should_replan(time.monotonic(), epos): - self._apply_slots(self._pursue_slots(gps, epos), epos) + + if self._should_replan(now, epos): + self._update_converge_command(gps, epos) def _tick_encircle(self): gps = self._gps_provider() epos = gps.get(self.enemy_id) + now = time.monotonic() + if not epos: + self._handle_enemy_missing(gps, now) return + + self._last_enemy_seen_at = now self._update_enemy_velocity(epos) - d = self._centroid_dist(gps, epos) - if d is None: + + if self._positions_for(self._friendly_ids, gps) is None: + return + + # 敵機持續移動，圓心跟著預測位置移動。 + # 若任何友機明顯脫離自己的 slot，回 CONVERGE 重新做曲線收斂。 + if not self._all_in_slots(gps, epos, tolerance=self.encircle_tolerance * 1.8): + self._set_phase(MissionPhase.CONVERGE) + self.status_message.emit("包圍幾何失配，重新收斂") + self._update_converge_command(gps, epos, force_assignment=False) + return + + if self._should_replan(now, epos): + self._update_encircle_command(gps, epos) + + def _tick_target_lost(self): + gps = self._gps_provider() + epos = gps.get(self.enemy_id) + if not epos: + return + + # 目標重新出現：不沿用過期軌跡，重新估速並回 CONVERGE。 + self._enemy_hist = [] + self._enemy_vel = (0.0, 0.0) + self._last_enemy_seen_at = time.monotonic() + self._update_enemy_velocity(epos) + self._set_phase(MissionPhase.CONVERGE) + self.status_message.emit("敵機定位恢復，重新建立包圍軌跡") + self._update_converge_command(gps, epos, force_assignment=True) + + # ------------------------------------------------------------------ 敵機遺失 / hold + + def _handle_enemy_missing(self, gps, now): + if self._last_enemy_seen_at <= 0.0: + self._last_enemy_seen_at = now + return + if now - self._last_enemy_seen_at < self.target_lost_timeout_sec: 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("敵機逃離，退回前進") + + self._hold_current_positions(gps) + self._set_phase(MissionPhase.TARGET_LOST) + self.status_message.emit("敵機定位逾時：友機保持目前位置") + + def _hold_current_positions(self, gps): + if self._merged_ex is None: + return + targets = {} + for did in self._friendly_ids: + p = gps.get(did) + if not p: + continue + targets[did] = ( + p['lat'], + p['lon'], + max(self.min_alt_floor, float(p.get('alt', self.formation_alt))), + ) + if len(targets) == len(self._friendly_ids): + self._merged_ex.update_plan(self._targets_to_plan(targets)) + + # ------------------------------------------------------------------ 敵機速度 / 預測中心 + + def _set_enemy_heading(self, heading_deg): + """以最短角度差低通更新敵機 heading，正確處理 359°/0° 邊界。""" + heading_deg = float(heading_deg) % 360.0 + if self._enemy_heading_deg is None: + self._enemy_heading_deg = heading_deg 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 - - # ------------------------------------------------------------- 前置攔截（估敵速） + err = ((heading_deg - self._enemy_heading_deg + 180.0) % 360.0) - 180.0 + self._enemy_heading_deg = ( + self._enemy_heading_deg + self.heading_smooth_alpha * err + ) % 360.0 + def _update_enemy_velocity(self, epos): - """用敵機 GPS 差分估水平速度 (east, north) m/s，視窗 enemy_vel_window_sec。""" + """更新敵機高度/heading，並以 GPS 差分估水平速度 (east, north) m/s。""" + try: + enemy_alt = float(epos.get('alt')) + except (TypeError, ValueError): + enemy_alt = None + if enemy_alt is not None and math.isfinite(enemy_alt): + self._enemy_alt = enemy_alt + + # heading convention：0°=North、90°=East、順時針增加。 + # 優先讀 heading/hdg/course；yaw 若像 radians 就自動轉 degree。 + raw_heading = None + for key in ('heading', 'hdg', 'course'): + try: + v = epos.get(key) + if v is not None and math.isfinite(float(v)): + v = float(v) + if key == 'hdg' and abs(v) > 360.0: + v *= 0.01 + raw_heading = v % 360.0 + break + except (TypeError, ValueError): + pass + if raw_heading is None: + try: + v = epos.get('yaw') + if v is not None and math.isfinite(float(v)): + v = float(v) + raw_heading = (math.degrees(v) if abs(v) <= 2.0 * math.pi + 0.2 else v) % 360.0 + except (TypeError, ValueError): + pass + + if raw_heading is not None: + self._set_enemy_heading(raw_heading) + 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): + + 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) + dx, dy = _ll_to_m(la1, lo1, la0, lo0) self._enemy_vel = (dx / dt, dy / dt) + # 若 provider 沒有 heading，以每次更新的水平航跡方向作持續備援； + # 不能只在第一次設定，否則敵機轉彎後 X 陣形不會跟著旋轉。 + if raw_heading is None: + evx, evy = self._enemy_vel + if math.hypot(evx, evy) > 0.3: + course_deg = math.degrees(math.atan2(evx, evy)) % 360.0 + self._set_enemy_heading(course_deg) + 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）。敵速尚未估到時退化為瞄當下位置（等同原純追尾）。""" + def _predict_enemy_center(self, gps, epos): + """ + 預測「包圍圓未來中心」，而不是給 leader 一個追擊點。 + lead time 取友機到敵機平均距離 / friendly_speed，並封頂。 + """ + gaps = [] + for did in self._friendly_ids: + p = gps.get(did) + if p: + gaps.append(_haversine(p['lat'], p['lon'], epos['lat'], epos['lon'])) + avg_gap = sum(gaps) / len(gaps) if gaps else 0.0 + raw_lead_t = min( + self.lead_time_cap_sec, + avg_gap / max(0.1, self.friendly_speed), + ) + + # 遠距離保留前置攔截；接近包圍圈時逐步把前置量淡出。 + # 這樣最終包圍圓會貼著敵機當下位置，不會因敵機速度 × lead time + # 讓視覺上/實際上看起來像半徑被拉到 20~30 m。 + if self.lead_fade_start <= self.lead_fade_end + 1e-6: + lead_scale = 0.0 if avg_gap <= self.lead_fade_end else 1.0 + elif avg_gap >= self.lead_fade_start: + lead_scale = 1.0 + elif avg_gap <= self.lead_fade_end: + lead_scale = 0.0 + else: + lead_scale = (avg_gap - self.lead_fade_end) / ( + self.lead_fade_start - self.lead_fade_end + ) + + lead_t = raw_lead_t * lead_scale 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: + plat, plon = _m_to_ll(evx * lead_t, evy * lead_t, epos['lat'], epos['lon']) + return plat, plon + + def _planning_center(self, gps, epos, reset=False): + """ + 對預測圓心做低通。只在真正產生命令/初始規劃時呼叫，避免同一 tick + 多次查詢時重複濾波。這能明顯降低 moving target/GPS 抖動造成的曲線折感。 + """ + raw = self._predict_enemy_center(gps, epos) + if reset or self._smoothed_center is None: + self._smoothed_center = raw + return raw + plat, plon = self._smoothed_center + dx, dy = _ll_to_m(raw[0], raw[1], plat, plon) + a = self.center_smooth_alpha + filt = _m_to_ll(dx * a, dy * a, plat, plon) + self._smoothed_center = filt + return filt + + # ------------------------------------------------------------------ slot 生成 / 高度 + + def _group_of(self, did): + if did in self.a_drone_ids: + return "A" + if did in self.b_drone_ids: + return "B" + return None + + def _group_ids(self, group): + src = self.a_drone_ids if group == "A" else self.b_drone_ids + friendly = set(self._friendly_ids) + return [d for d in src if d in friendly] + + def _layer_alt(self, group): + """以敵機即時高度為中心：A 在上層、B 在下層。""" + base_alt = ( + self._enemy_alt + if self._enemy_alt is not None + else self.formation_alt + ) + half = self.alt_layer_gap / 2.0 + lower = max(self.min_alt_floor, base_alt - half) + # 敵機高度過低而觸發最低高度限制時，仍保留完整 A/B 垂直間隔。 + upper = max(self.min_alt_floor, base_alt + half, lower + self.alt_layer_gap) + return upper if group == "A" else lower + + @staticmethod + def _position_alt(pos): + """讀取有效相對高度；缺值或非有限值回傳 None。""" + if not pos: 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] # 左垂直 + try: + alt = float(pos.get('alt')) + except (TypeError, ValueError): + return None + return alt if math.isfinite(alt) else None + + def _cross_layer_vertical_hold_ids(self, gps): + """ + 找出必須先原地垂直分層的 A/B 友機。 + + 只有 A/B 水平距離小於 cross_layer_hold_distance 時才介入；待實際 + 高度符合 A 在 B 上方，且垂直差接近規劃層差後，才解除水平鎖定。 + """ + hold = set() + a_ids = [d for d in self._group_ids("A") if gps.get(d)] + b_ids = [d for d in self._group_ids("B") if gps.get(d)] + required_gap = min( + self._layer_alt("A") - self._layer_alt("B"), + self.cross_layer_min_vertical_gap, + ) + + for aid in a_ids: + apos = gps[aid] + for bid in b_ids: + bpos = gps[bid] + horizontal = _haversine( + apos['lat'], apos['lon'], bpos['lat'], bpos['lon'] + ) + if horizontal >= self.cross_layer_hold_distance: + continue + a_alt = self._position_alt(apos) + b_alt = self._position_alt(bpos) + # 高度缺失時採保守策略：不允許近距離跨層水平接近。 + if a_alt is None or b_alt is None or a_alt - b_alt < required_gap: + hold.update((aid, bid)) + return hold + + def _apply_cross_layer_vertical_gate(self, targets, gps): + """把跨層近距離且未完成垂直分離的水平目標鎖在當前位置。""" + hold = self._cross_layer_vertical_hold_ids(gps) + for did in hold: + p = gps.get(did) + if not p or did not in targets: + continue + targets[did] = (p['lat'], p['lon'], self._layer_alt(self._group_of(did))) + self._motion_dirs.pop(did, None) + + if hold != self._vertical_hold_ids: + if hold: + labels = ", ".join(sorted(hold)) + self.status_message.emit( + f"A/B 近距離：{labels} 先垂直分層，再恢復水平移動" + ) + elif self._vertical_hold_ids: + self.status_message.emit("A/B 垂直分層完成，恢復水平移動") + self._vertical_hold_ids = hold + return targets + + def _slot_alt(self, slot_index): + group = self._slot_groups.get(slot_index, "A") + return self._layer_alt(group) + + def _circle_slots_for_center(self, center, n): + """ + 以敵機機體座標系建立真正的 X 形包圍。 + + body-frame： + forward = 敵機 heading 0° 軸 + right = 敵機右側 90° 軸 + + 四機、A/B 各兩機： + A 層：front-right、rear-left + B 層：front-left、rear-right + + heading=0° 時，四個 slot 固定落在 NE / NW / SW / SE 四個對角方向。 + """ + clat, clon = center 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 + self._slot_groups = {} + + heading_deg = self._enemy_heading_deg if self._enemy_heading_deg is not None else 0.0 + h = math.radians(heading_deg) + + # Local map frame: x=East, y=North + # heading: 0°=North, 90°=East, clockwise + fx, fy = math.sin(h), math.cos(h) + rx, ry = math.cos(h), -math.sin(h) + s = 1.0 / math.sqrt(2.0) + + body_dirs = { + "front_right": ((fx + rx) * s, (fy + ry) * s), + "front_left": ((fx - rx) * s, (fy - ry) * s), + "rear_left": ((-fx - rx) * s, (-fy - ry) * s), + "rear_right": ((-fx + rx) * s, (-fy + ry) * s), + } + + group_names = { + "A": ("front_right", "rear_left"), + "B": ("front_left", "rear_right"), + } + + idx = 0 + for group in ("A", "B"): + gids = self._group_ids(group) + m = len(gids) + if m <= 0: + continue + + if m == 2: + dirs = [body_dirs[name] for name in group_names[group]] + else: + base_deg = 45.0 if group == "A" else -45.0 + dirs = [] + for j in range(m): + off = math.radians(base_deg + 360.0 * j / m) + bx = math.sin(off) + by = math.cos(off) + dx = bx * rx + by * fx + dy = bx * ry + by * fy + dirs.append((dx, dy)) + + for dx, dy in dirs: + x = self.circle_radius * dx + y = self.circle_radius * dy + lat, lon = _m_to_ll(x, y, clat, clon) + slots[idx] = (lat, lon, self._layer_alt(group)) + self._slot_groups[idx] = group + idx += 1 - 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 _lane_offset_for(self, did): + """同一層內給每架固定的同心圓半徑車道，避免車道在飛行中交換。""" + group = self._group_of(did) + gids = self._group_ids(group) + if did not in gids or len(gids) <= 1: + return 0.0 + i = gids.index(did) + return (i - (len(gids) - 1) / 2.0) * self.layer_lane_spacing + + # ------------------------------------------------------------------ Bezier 路徑 + + def _assignment_target_angles(self, assignment, gps, center, slots): + """ + 把 slot 角度展開成與友機目前角度相同的連續角域。 + 當 assignment 保持環形順序時，所有友機的角度插值全程保持順序， + 不會因為跨過 -pi/pi 邊界而突然反向或互相超車。 + """ + rows = [] + for did in self._friendly_ids: + p = gps.get(did) + if not p or did not in assignment: + return None + x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1]) + rows.append((math.atan2(y, x), did)) + rows.sort(key=lambda z: z[0]) + + src = [r[0] for r in rows] + raw_targets = [] + prev = None + for _, did in rows: + slot = slots[assignment[did]] + sx, sy = _ll_to_m(slot[0], slot[1], center[0], center[1]) + th = math.atan2(sy, sx) + if prev is not None: + while th <= prev: + th += 2.0 * math.pi + raw_targets.append(th) + prev = th + + # 全體只加同一個 2pi 倍數，讓總旋轉量最小，同時保持拓樸順序。 + best = None + best_err = float('inf') + for k in range(-2, 3): + cand = [th + k * 2.0 * math.pi for th in raw_targets] + err = sum((cand[i] - src[i]) ** 2 for i in range(len(src))) + if err < best_err: + best_err, best = err, cand + return {rows[i][1]: best[i] for i in range(len(rows))} + + def _trajectory_for(self, did, slot_index, gps, center, slots, assignment=None): + """ + C2 平滑、近似最短路徑的 Cartesian 收斂軌跡。 + + 舊版使用極座標 theta/r 插值，當起點與 slot 的方位角差較大時， + 即使沒有交叉，也可能沿敵機外圍繞出很大的弧。 + + 新版改成： + base(t) = P0 -> P3 的 quintic smoothstep + offset(t) = 小幅「向外」bump + + 因此主體接近兩點間的直接路徑，只保留少量發散來增加機間分離； + crossing / safe separation 仍由全局 assignment 的硬限制負責。 + quintic smoothstep 使起終點的一、二階導數為 0，保持平滑。 + """ + p = gps.get(did) + if not p: + return None + if assignment is None: + assignment = self._assignment + + clat, clon = center + p0 = _ll_to_m(p['lat'], p['lon'], clat, clon) + + slot = slots[slot_index] + p3 = _ll_to_m(slot[0], slot[1], clat, clon) + + dx = p3[0] - p0[0] + dy = p3[1] - p0[1] + dist_to_slot = math.hypot(dx, dy) + + # 外擴方向：以敵機/包圍中心為原點，由目前位置向外。 + # 若目前位置非常接近中心，退化成與直達方向垂直的方向。 + ox, oy = _normalize(p0[0], p0[1], fallback=_normalize(-dy, dx)) + + # 不再隨距離大幅外擴。 + # 最多 spread_distance（預設 3m），近距離時再自動縮小。 + bump = min( + self.spread_distance, + max(0.0, 0.08 * dist_to_slot), + ) + + points = [] + for i in range(self.path_samples): + t = i / (self.path_samples - 1) + q = _smoothstep5(t) + + # 近似直達的 C2 smoothstep 主路徑。 + bx = p0[0] + dx * q + by = p0[1] + dy * q + + # 小幅外擴：首尾為 0，中間最大。 + # 使用 q^2(1-q)^2 讓 offset 在兩端也非常柔順。 + shape = 16.0 * q * q * (1.0 - q) * (1.0 - q) + x = bx + ox * bump * shape + y = by + oy * bump * shape + points.append((x, y)) - def _slots_to_plan(self, slots): return { - 'drone_ids': list(self._merged_ids), - 'waypoints': [[slots[d]] for d in self._merged_ids], - 'rendezvous_indices': [], + 'p0': points[0], + 'p3': points[-1], + 'points': points, + 'length': _polyline_length(points), } - # ------------------------------------------------------------- 內部 - def _centroid_dist(self, gps, epos): - m_pos = self._positions_for(self._merged_ids, gps) - if m_pos is None: + def _lookahead_target(self, did, slot_index, gps, center, slots): + traj = self._trajectory_for( + did, slot_index, gps, center, slots, assignment=self._assignment + ) + if traj 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: + dist_to_slot = math.hypot( + traj['p3'][0] - traj['p0'][0], + traj['p3'][1] - traj['p0'][1], + ) + if dist_to_slot <= self.final_direct_distance: + slot = slots[slot_index] + return slot[0], slot[1], slot[2] + + # 固定「公尺」前視，比固定參數 t 更不容易因曲線長短改變而突然加減速。 + want = min(self.lookahead_distance, max(6.0, 0.70 * dist_to_slot)) + acc = 0.0 + chosen = traj['points'][-1] + for a, b in zip(traj['points'], traj['points'][1:]): + seg = math.hypot(b[0] - a[0], b[1] - a[1]) + if acc + seg >= want and seg > 1e-9: + u = (want - acc) / seg + chosen = (a[0] + u * (b[0] - a[0]), a[1] + u * (b[1] - a[1])) + break + acc += seg + + lat, lon = _m_to_ll(chosen[0], chosen[1], center[0], center[1]) + cur_alt = float(gps[did].get('alt', self.formation_alt)) + target_alt = slots[slot_index][2] + # 高度用距離比例平滑，但近場直接交給 slot。 + frac = min(1.0, want / max(1e-6, traj['length'])) + q = _smoothstep5(frac) + alt = cur_alt + q * (target_alt - cur_alt) + return lat, lon, max(self.min_alt_floor, alt) + + def _converge_targets(self, gps, center, slots, assignment): + targets = {} + for did in self._friendly_ids: + sidx = assignment.get(did) + if sidx is None: + return None + target = self._lookahead_target(did, sidx, gps, center, slots) + if target is None: + return None + targets[did] = target + return targets + + def _separation_correction_xy(self, did, gps, ref_lat, ref_lon): + """目前太靠近其他友機時，計算小幅排斥修正；不取代前進目標，只疊加在其上。""" + p = gps.get(did) + if not p: + return 0.0, 0.0 + x, y = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) + cx = cy = 0.0 + my_group = self._group_of(did) + for j, oid in enumerate(self._friendly_ids): + if oid == did or self._group_of(oid) != my_group: + continue + op = gps.get(oid) + if not op: + continue + ox, oy = _ll_to_m(op['lat'], op['lon'], ref_lat, ref_lon) + dx, dy = x - ox, y - oy + d = math.hypot(dx, dy) + # 8m 是硬安全門檻，但隊形不等到 8m 才開始整理； + # 在 formation_spacing（預設 12m）內就逐步往外展開。 + if d >= self.formation_spacing: + continue + if d < 1e-6: + # 完全重合時用固定但每機不同的方向，避免除零。 + idx = self._friendly_ids.index(did) + th = 2.0 * math.pi * idx / max(1, len(self._friendly_ids)) + ux, uy = math.cos(th), math.sin(th) + d = 0.0 + else: + ux, uy = dx / d, dy / d + # 以「隊形希望間距」而非硬安全距離算修正，因此會更早開始展開。 + deficit = self.formation_spacing - d + closeness = deficit / max(0.1, self.formation_spacing) + boost = 1.0 + 0.7 * closeness + mag = 0.5 * self.separation_correction_gain * deficit * boost + cx += ux * mag + cy += uy * mag + + n = math.hypot(cx, cy) + if n > self.separation_correction_max > 0.0: + k = self.separation_correction_max / n + cx *= k + cy *= k + return cx, cy + + def _segments_cross_for_targets(self, gps, targets, ref_lat, ref_lon): + """只檢查『當前位置 -> 下一個 lookahead』短線段是否彼此相交。""" + ids = [d for d in self._friendly_ids if d in targets and gps.get(d)] + segs = {} + for did in ids: + p = gps[did] + a = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) + t = targets[did] + b = _ll_to_m(t[0], t[1], ref_lat, ref_lon) + segs[did] = (a, b) + for i in range(len(ids)): + for j in range(i + 1, len(ids)): + if self._group_of(ids[i]) != self._group_of(ids[j]): + continue + if _segments_intersect(segs[ids[i]][0], segs[ids[i]][1], + segs[ids[j]][0], segs[ids[j]][1]): + return True + return False + + def _layer_conflict_flags(self, gps, center, slots, assignment): + """判斷各高度層是否需要啟用同心圓車道。跨層完全不互相觸發。""" + flags = {"A": False, "B": False} + ref_lat, ref_lon = center + for group in ("A", "B"): + gids = [d for d in self._group_ids(group) if d in assignment and gps.get(d)] + for i in range(len(gids)): + di = gids[i] + pi = gps[di] + aix = _ll_to_m(pi['lat'], pi['lon'], ref_lat, ref_lon) + si = slots[assignment[di]] + bix = _ll_to_m(si[0], si[1], ref_lat, ref_lon) + for j in range(i + 1, len(gids)): + dj = gids[j] + pj = gps[dj] + ajx = _ll_to_m(pj['lat'], pj['lon'], ref_lat, ref_lon) + sj = slots[assignment[dj]] + bjx = _ll_to_m(sj[0], sj[1], ref_lat, ref_lon) + d = math.hypot(aix[0] - ajx[0], aix[1] - ajx[1]) + if d < self.layer_avoid_distance or _segments_intersect(aix, bix, ajx, bjx): + flags[group] = True + break + if flags[group]: + break + return flags + + def _concentric_lane_carrot(self, did, p, slot, center, dist_to_slot): + """ + 同層避讓用的同心圓 carrot。 + - 每架固定一條暫時半徑車道。 + - 沿角向前進，同時平滑靠近該車道半徑。 + - 接近最終 slot 時 lane offset 淡出為 0，回到共同包圍半徑。 + """ + ref_lat, ref_lon = center + px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) + sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon) + r = max(0.5, math.hypot(px, py)) + th = math.atan2(py, px) + sth = math.atan2(sy, sx) + dth = _wrap_pi(sth - th) + + # 遠處完整使用分層車道；進入 slot 前逐步收回共同半徑。 + fade = min(1.0, max(0.0, + (dist_to_slot - self.final_direct_distance) / + max(1.0, self.layer_lane_fade_distance - self.final_direct_distance) + )) + lane_r = max(2.0, self.circle_radius + self._lane_offset_for(did) * fade) + + horizon = min(self.lookahead_distance, max(4.0, dist_to_slot)) + # 約 70% 用於角向前進、30% 用於半徑收斂，避免突然徑向橫切。 + arc_budget = 0.72 * horizon + radial_budget = 0.35 * horizon + th_step = min(abs(dth), arc_budget / max(2.0, r)) + nth = th + (1.0 if dth >= 0.0 else -1.0) * th_step + dr = max(-radial_budget, min(radial_budget, lane_r - r)) + nr = max(2.0, r + dr) + + tx = nr * math.cos(nth) + ty = nr * math.sin(nth) + return tx, ty + + def _moving_converge_targets(self, gps, center, slots, assignment): + """ + A/B 分層持續移動控制： + - A/B 高度層跟隨敵機即時高度。 + - 跨層近距離且垂直間隔不足時，先鎖住水平位置完成分層。 + - 同層若過近或直達線可能交叉，切換到不同半徑的同心圓車道。 + - 非跨層垂直分離狀態時，持續保持前方 carrot。 + """ + ref_lat, ref_lon = center + layer_avoid = self._layer_conflict_flags(gps, center, slots, assignment) + raw = {} + + for did in self._friendly_ids: + p = gps.get(did) + sidx = assignment.get(did) + if not p or sidx is None: + return None + slot = slots[sidx] + px, py = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) + sx, sy = _ll_to_m(slot[0], slot[1], ref_lat, ref_lon) + vx, vy = sx - px, sy - py + dist = math.hypot(vx, vy) + + if dist <= self.final_direct_distance: + raw[did] = (slot[0], slot[1], self._layer_alt(self._group_of(did))) + continue + + group = self._group_of(did) + if layer_avoid.get(group, False): + tx, ty = self._concentric_lane_carrot(did, p, slot, center, dist) + dux, duy = _normalize(tx - px, ty - py) + else: + dux, duy = _normalize(vx, vy) + horizon = min(self.lookahead_distance, dist) + tx, ty = px + dux * horizon, py + duy * horizon + + # 同層近距離排斥仍保留，但只取側向，不會抵消前進分量。 + rx, ry = self._separation_correction_xy(did, gps, ref_lat, ref_lon) + dot = rx * dux + ry * duy + lx, ly = rx - dot * dux, ry - dot * duy + ln = math.hypot(lx, ly) + cap = min(self.separation_correction_max, + self.max_lateral_ratio * self.lookahead_distance) + if ln > cap > 0.0: + k = cap / ln + lx *= k; ly *= k + tx += lx + ty += ly + + ndx, ndy = _normalize(tx - px, ty - py, fallback=(dux, duy)) + prev = self._motion_dirs.get(did) + if prev is not None: + a = self.direction_smooth_alpha + ndx, ndy = _normalize(prev[0]*(1-a)+ndx*a, + prev[1]*(1-a)+ndy*a, + fallback=(ndx, ndy)) + h = min(self.lookahead_distance, max(4.0, dist)) + tx, ty = px + ndx*h, py + ndy*h + self._motion_dirs[did] = (ndx, ndy) + + lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon) + target_alt = self._layer_alt(group) + cur_alt = float(p.get('alt', target_alt)) + alt = cur_alt + 0.35 * (target_alt - cur_alt) + raw[did] = (lat, lon, max(self.min_alt_floor, alt)) + + return self._apply_cross_layer_vertical_gate(raw, gps) + + def _recovery_spread_targets(self, gps): + """ + 找不到安全 slot assignment 時的短距離預分散。 + + 每架只沿「友軍形心 -> 自己」方向往外移 recovery_spread_step； + 不繞敵機、不做大圓，只把原本過密的起始幾何稍微拉開。 + """ + pts = [] + for did in self._friendly_ids: p = gps.get(did) if not p: return None - out.append((p['lat'], p['lon'], p.get('alt', 0.0))) + pts.append((p['lat'], p['lon'])) + if not pts: + return None + + ref_lat = sum(p[0] for p in pts) / len(pts) + ref_lon = sum(p[1] for p in pts) / len(pts) + xy = {} + for did in self._friendly_ids: + p = gps[did] + xy[did] = _ll_to_m(p['lat'], p['lon'], ref_lat, ref_lon) + cx = sum(v[0] for v in xy.values()) / len(xy) + cy = sum(v[1] for v in xy.values()) / len(xy) + + targets = {} + n = len(self._friendly_ids) + for idx, did in enumerate(self._friendly_ids): + x, y = xy[did] + ox, oy = x - cx, y - cy + if math.hypot(ox, oy) < 1e-6: + th = 2.0 * math.pi * idx / max(1, n) + ox, oy = math.cos(th), math.sin(th) + ox, oy = _normalize(ox, oy) + tx = x + ox * self.recovery_spread_step + ty = y + oy * self.recovery_spread_step + lat, lon = _m_to_ll(tx, ty, ref_lat, ref_lon) + alt = max( + self.min_alt_floor, + float(gps[did].get('alt', self.formation_alt)), + ) + targets[did] = (lat, lon, alt) + return targets + + # ------------------------------------------------------------------ assignment / crossing / separation + + def _assignment_cost(self, assignment, gps, center, slots): + """ + 評估一個完整 assignment。 + + crossing 仍是硬限制。水平間距改成「漸進安全」： + - 若兩機起始已 >= safe_separation，後續不得低於 safe_separation。 + - 若起始本來 < safe_separation，不要求瞬間跳到 8m；只要求路徑不要 + 比起始距離再惡化超過 initial_sep_tolerance，並且永遠不得低於 + emergency_separation。 + + 這避免四機原本只隔 4~7m 時，所有 assignment 在第一個 sample 就被 + 8m 硬門檻全部淘汰。 + """ + trajectories = {} + total = 0.0 + + for did in self._friendly_ids: + sidx = assignment[did] + traj = self._trajectory_for(did, sidx, gps, center, slots, assignment=assignment) + if traj is None: + return float('inf'), True, 0, 0.0 + trajectories[did] = traj + total += traj['length'] + + old = self._assignment.get(did) + if old is not None and old != sidx: + total += self.slot_switch_penalty + + crossing_count = 0 + separation_violation = False + min_sep = float('inf') + ids = self._friendly_ids + + for i in range(len(ids)): + for j in range(i + 1, len(ids)): + # A/B 高度層不同：只對同層做水平 crossing / separation 約束。 + if self._group_of(ids[i]) != self._group_of(ids[j]): + continue + a = trajectories[ids[i]]['points'] + b = trajectories[ids[j]]['points'] + + # 幾何 crossing：從頭到尾都不允許真正穿越。 + crossed = False + for a0, a1 in zip(a, a[1:]): + if crossed: + break + for b0, b1 in zip(b, b[1:]): + if _segments_intersect(a0, a1, b0, b1): + crossed = True + break + if crossed: + crossing_count += 1 + total += self.crossing_penalty + + # 起始實際水平距離。 + initial_sep = math.hypot(a[0][0] - b[0][0], a[0][1] - b[0][1]) + + # 已經足夠遠：維持完整 8m；本來較近：不能再明顯靠得更近。 + if initial_sep >= self.safe_separation: + required_sep = self.safe_separation + else: + required_sep = max( + self.emergency_separation, + initial_sep - self.initial_sep_tolerance, + ) + + # 同 normalized t 的預測水平距離。略過 t=0，因為那是既成狀態。 + pair_min = float('inf') + for k in range(1, min(len(a), len(b))): + d = math.hypot(a[k][0] - b[k][0], a[k][1] - b[k][1]) + pair_min = min(pair_min, d) + min_sep = min(min_sep, pair_min) + + if pair_min < required_sep: + separation_violation = True + ratio = (required_sep - pair_min) / max(0.1, required_sep) + total += self.separation_penalty * (1.0 + ratio) + + if min_sep == float('inf'): + min_sep = 9999.0 + + conflict = crossing_count > 0 or separation_violation + return total, conflict, crossing_count, min_sep + + def _cyclic_assignments(self, gps, center, slots): + """A/B 各自在自己的高度層做 cyclic slot rotation，再取笛卡兒積組合。""" + per_group = [] + for group in ("A", "B"): + ids = self._group_ids(group) + if not ids: + continue + rows = [] + for did in ids: + p = gps.get(did) + if not p: + return [] + x, y = _ll_to_m(p['lat'], p['lon'], center[0], center[1]) + rows.append((math.atan2(y, x), did)) + rows.sort(key=lambda z: z[0]) + ordered_ids = [d for _, d in rows] + + slot_rows = [] + for sidx, sp in slots.items(): + if self._slot_groups.get(sidx) != group: + continue + x, y = _ll_to_m(sp[0], sp[1], center[0], center[1]) + slot_rows.append((math.atan2(y, x), sidx)) + slot_rows.sort(key=lambda z: z[0]) + ordered_slots = [s for _, s in slot_rows] + if len(ordered_slots) != len(ordered_ids): + return [] + + group_candidates = [] + m = len(ordered_ids) + for shift in range(m): + group_candidates.append({ + ordered_ids[i]: ordered_slots[(i + shift) % m] + for i in range(m) + }) + per_group.append(group_candidates) + + if not per_group: + return [] + out = [] + for combo in product(*per_group): + merged = {} + for part in combo: + merged.update(part) + out.append(merged) + return out + + def _group_permutation_assignments(self, slots): + """只允許 A->A slots、B->B slots；小隊內可做完整 permutation 保底。""" + per_group = [] + for group in ("A", "B"): + ids = self._group_ids(group) + if not ids: + continue + sidxs = [s for s in slots if self._slot_groups.get(s) == group] + if len(ids) != len(sidxs): + return [] + if len(ids) > 6: + return [] + gc = [] + for perm in permutations(sidxs): + gc.append({did: perm[i] for i, did in enumerate(ids)}) + per_group.append(gc) + if not per_group: + return [] + out=[] + for combo in product(*per_group): + d={} + for part in combo: d.update(part) + out.append(d) return out - def _merged_ids_leader_first(self, gps, epos): - """A+B 合併；A 中離敵最近者當領隊(rank0)，其餘 A 維持原序，再接 B。""" - def dist(did): + def _choose_assignment(self, gps, center, slots, force=False): + """ + Crossing 是硬限制，不再只是 penalty。 + + 優先搜尋保持環形順序的 n 個 rotation；只有找不到安全解時才擴大到全部 + permutation。任何 crossings > 0 或違反『漸進安全間距』的方案都不採用。 + 若目前 assignment 仍安全就鎖住，不為了省幾公尺頻繁換位。 + """ + ids = list(self._friendly_ids) + n = len(ids) + bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')} + if n == 0 or len(slots) != n: + return None, bad + + # 一旦開始移動，只要目前 assignment 的「幾何路徑沒有交叉」就鎖定。 + # 暫時的間距不足交給 moving separation correction 處理，不因此突然換 slot。 + # 這可避免飛行途中目標位置跳到另一側而先煞停再重新排隊。 + if self._assignment and not force: + cost, conflict, crossings, min_sep = self._assignment_cost( + self._assignment, gps, center, slots + ) + if crossings == 0: + return dict(self._assignment), { + 'cost': cost, 'conflict': conflict, + 'crossings': 0, 'min_sep': min_sep, + } + + candidates = self._cyclic_assignments(gps, center, slots) + # cyclic 不足時只擴大到「各層內」的 permutation，不允許 A/B 交換高度層。 + all_perm = self._group_permutation_assignments(slots) + seen = {tuple(c.get(d) for d in ids) for c in candidates} + candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen] + + valid = [] + for assignment in candidates: + cost, conflict, crossings, min_sep = self._assignment_cost( + assignment, gps, center, slots + ) + # 幾何不交叉 + 漸進安全間距，兩者都是硬條件。 + if crossings == 0 and not conflict: + valid.append((cost, -min_sep, assignment, { + 'cost': cost, 'conflict': False, + 'crossings': 0, 'min_sep': min_sep, + })) + + if not valid: + return None, bad + valid.sort(key=lambda x: (x[0], x[1])) + return valid[0][2], valid[0][3] + + def _choose_progress_assignment(self, gps, center, slots): + """ + 找不到完整安全間距方案時的移動中保底。 + + crossing 仍是硬限制：只從 0-crossing assignment 中選最低風險方案。 + 允許暫時低於 safe_separation，之後由 moving separation correction + 在持續前進的同時把距離拉開。 + """ + ids = list(self._friendly_ids) + n = len(ids) + bad = {'conflict': True, 'crossings': 999, 'min_sep': 0.0, 'cost': float('inf')} + if n == 0 or len(slots) != n: + return None, bad + + candidates = self._cyclic_assignments(gps, center, slots) + all_perm = self._group_permutation_assignments(slots) + seen = {tuple(c.get(d) for d in ids) for c in candidates} + candidates += [c for c in all_perm if tuple(c.get(d) for d in ids) not in seen] + + valid = [] + for assignment in candidates: + cost, conflict, crossings, min_sep = self._assignment_cost( + assignment, gps, center, slots + ) + if crossings != 0: + continue + # 先偏好較大的預測最小間距，再看路徑成本；不接受幾何交叉。 + valid.append((-min_sep, cost, assignment, { + 'cost': cost, + 'conflict': conflict, + 'crossings': 0, + 'min_sep': min_sep, + })) + + if not valid: + return None, bad + valid.sort(key=lambda x: (x[0], x[1])) + return valid[0][2], valid[0][3] + + def _greedy_assignment(self, gps, center, slots): + """大於 7 機時的保底 nearest-slot greedy。""" + remain = set(slots.keys()) + assignment = {} + for did in self._friendly_ids: 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) + if not p: + continue + best_s = min( + remain, + key=lambda s: _haversine( + p['lat'], p['lon'], slots[s][0], slots[s][1] + ), + ) + assignment[did] = best_s + remain.remove(best_s) + return assignment + + # ------------------------------------------------------------------ command 更新 + + def _update_converge_command(self, gps, epos, force_assignment=False): + if self._merged_ex is None: + return + + center = self._planning_center(gps, epos) + slots = self._circle_slots_for_center(center, len(self._friendly_ids)) + new_assignment, info = self._choose_assignment( + gps, center, slots, force=force_assignment + ) + if not new_assignment: + # 若任務已經在移動中，優先沿用既有 slot，不在半路 hold。 + # 只有「一開始尚未建立任何 assignment」時才找 provisional 0-crossing 配置。 + if self._assignment: + new_assignment = dict(self._assignment) + _, conflict, crossings, min_sep = self._assignment_cost( + new_assignment, gps, center, slots + ) + info = { + 'cost': 0.0, 'conflict': conflict, + 'crossings': crossings, 'min_sep': min_sep, + } + else: + new_assignment, info = self._choose_progress_assignment(gps, center, slots) + if not new_assignment: + # 尚未開始飛行時若連 0-crossing topology 都沒有，才暫停。 + self._hold_current_positions(gps) + self.status_message.emit("起始暫停：目前連 0-crossing 路徑都不存在") + return + + changed = new_assignment != self._assignment + if changed: + self._assignment = new_assignment + self._last_assignment_change_at = time.monotonic() + msg = ( + f"slot 重新分配：cross={info['crossings']}，" + f"預測最小間距={info['min_sep']:.1f} m" + ) + _log("INFO", f"[orchestrator] {msg}") + self.status_message.emit(msg) + + self._last_assignment_had_conflict = info['conflict'] + targets = self._moving_converge_targets(gps, center, slots, self._assignment) + if targets is None: + return + + self._merged_ex.update_plan(self._targets_to_plan(targets)) + self._last_predicted_center = center + self._last_slots = slots + self._mark_replanned(epos) + + def _update_encircle_command(self, gps, epos, force=False): + if self._merged_ex is None: + return + now = time.monotonic() + if not force and not self._should_replan(now, epos): + return + + center = self._planning_center(gps, epos) + slots = self._circle_slots_for_center(center, len(self._friendly_ids)) + + # ENCIRCLE 內原則上不交換 slot，以免四機繞圈互換造成穿越。 + # 只有現有 assignment 已無效才重新建立。 + if not self._assignment or any(d not in self._assignment for d in self._friendly_ids): + assignment, _ = self._choose_assignment(gps, center, slots, force=True) + if assignment: + self._assignment = assignment + self._last_assignment_change_at = now + + targets = { + did: slots[self._assignment[did]] + for did in self._friendly_ids + if did in self._assignment + } + if len(targets) != len(self._friendly_ids): + return + + targets = self._apply_cross_layer_vertical_gate(targets, gps) + + self._merged_ex.update_plan(self._targets_to_plan(targets)) + self._last_predicted_center = center + self._last_slots = slots + self._mark_replanned(epos) + + def _targets_to_plan(self, targets): + return { + 'drone_ids': list(self._friendly_ids), + 'waypoints': [[targets[d]] for d in self._friendly_ids], + 'rendezvous_indices': [], + } + + # ------------------------------------------------------------------ 包圍完成判斷 + + def _all_in_slots(self, gps, epos, tolerance=None): + if not self._assignment or not self._friendly_ids: + return False + tolerance = self.encircle_tolerance if tolerance is None else float(tolerance) + + # 水平 slot 即使已到達，只要近距離 A/B 尚未建立足夠垂直差，仍不可 + # 宣告包圍完成或提前解除「先垂直、後水平」保護。 + if self._cross_layer_vertical_hold_ids(gps): + return False + + center = self._last_predicted_center or self._predict_enemy_center(gps, epos) + slots = self._circle_slots_for_center(center, len(self._friendly_ids)) + + for did in self._friendly_ids: + p = gps.get(did) + sidx = self._assignment.get(did) + if not p or sidx is None: + return False + slot = slots[sidx] + d = _haversine(p['lat'], p['lon'], slot[0], slot[1]) + if d > tolerance: + return False + current_alt = self._position_alt(p) + if (current_alt is None + or abs(current_alt - slot[2]) > self.cross_layer_alt_tolerance): + return False + return True + + # ------------------------------------------------------------------ replan def _mark_replanned(self, epos): self._last_replan_at = time.monotonic() @@ -424,35 +1763,63 @@ class MissionOrchestrator(QObject): 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) + 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 + ) + + # ------------------------------------------------------------------ helpers + + 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 + + @staticmethod + def _unique_keep_order(items): + seen = set() + out = [] + for x in items: + if x in seen: + continue + seen.add(x) + out.append(x) + return out def _set_phase(self, phase): if phase == self.phase: return self.phase = phase - _log("INFO", f"[orchestrator] phase → {phase.value}") + _log("INFO", f"[orchestrator] phase -> {phase.value}") self.phase_changed.emit(phase.value) +# --------------------------------------------------------------------------- +# Engagement Panel + class EngagementPanel(QWidget): """ 交戰控制面板。 - - 敵機下拉 + 即時「敵機↔最近友機」距離 - - A/B 組指派下拉 - - 階段顯示、重編隊 / 停止 按鈕 - - 偵察飛行用既有的群組執行按鈕控制，這裡不含「開始」。面板只發訊號，邏輯在 GUI/orchestrator。 + - 敵機下拉 + 即時敵機到最近友機距離 + - A/B 僅用於偵察前的群組指定 + - 偵測確認後由 MissionOrchestrator 合併為無角色 friendly 集合 + - 速度比例 UI 保留，方便既有 GUI 發 DO_CHANGE_SPEED """ - enemy_changed = pyqtSignal(str) # drone_id（未選擇=空字串） - confirm_requested = pyqtSignal() # 重編隊 + enemy_changed = pyqtSignal(str) + 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) + groups_changed = pyqtSignal(str, str) + apply_speed_requested = pyqtSignal(float, float) def __init__(self, parent=None, r_detect=DEFAULT_R_DETECT, friendly_speed=DEFAULT_FRIENDLY_SPEED): @@ -462,10 +1829,11 @@ class EngagementPanel(QWidget): self._enemy_id = "" self._suppress_enemy = False self._suppress_group = False - self._group_sig = None # 目前下拉內容的簽章，變了才重建（避免 5Hz 狂刷） + self._group_sig = None self._build_ui() # ------------------------------------------------------------------ UI + def _build_ui(self): root = QVBoxLayout(self) root.setContentsMargins(12, 12, 12, 12) @@ -474,12 +1842,22 @@ class EngagementPanel(QWidget): title = QLabel("交戰設定") title.setStyleSheet( - "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;") + "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;" + ) root.addWidget(title) + mode_hint = QLabel( + "偵測確認後：A/B 跟隨敵機高度分層；近距離先垂直後水平，" + "同層以同心圓路徑避讓" + ) + mode_hint.setWordWrap(True) + mode_hint.setStyleSheet("color: #888; font-size: 11px;") + root.addWidget(mode_hint) + combo_css = ( "QComboBox { background-color: #333; color: #EEE; border: 1px solid #555;" - " border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }") + " border-radius: 3px; padding: 3px 6px; font-size: 12px; min-width: 120px; }" + ) enemy_row = QHBoxLayout() enemy_row.setSpacing(8) @@ -520,11 +1898,11 @@ class EngagementPanel(QWidget): 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; }") + " font-size: 12px; min-width: 70px; }" + ) speed_row = QHBoxLayout() speed_row.setSpacing(8) sl = QLabel("友機速度：") @@ -536,6 +1914,7 @@ class EngagementPanel(QWidget): 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() @@ -544,6 +1923,7 @@ class EngagementPanel(QWidget): 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) @@ -556,7 +1936,8 @@ class EngagementPanel(QWidget): 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; }") + " 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;") @@ -575,7 +1956,8 @@ class EngagementPanel(QWidget): self.distance_label = QLabel("敵機 ↔ 最近友機：—") self.distance_label.setStyleSheet( - "color: #AAA; font-size: 13px; padding: 4px 2px;") + "color: #AAA; font-size: 13px; padding: 4px 2px;" + ) root.addWidget(self.distance_label) hint = QLabel(f"（偵測範圍 R_detect = {self.r_detect:.0f} m）") @@ -584,28 +1966,39 @@ class EngagementPanel(QWidget): self.phase_label = QLabel("階段：待機") self.phase_label.setStyleSheet( - "color: #64B5F6; font-size: 13px; font-weight: bold; padding: 4px 2px;") + "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_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 = 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.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 + # ------------------------------------------------------------------ 對外 API + def current_enemy(self): return self._enemy_id or None @@ -618,17 +2011,19 @@ class EngagementPanel(QWidget): 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 狂刷。""" + """group_labels: list[(group_id, display_text)]。""" 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() @@ -638,11 +2033,35 @@ class EngagementPanel(QWidget): 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_configuration(self, enemy_id, a_group_id, b_group_id): + """由 GUI 程式化選定敵機與 A/B 群組，並發出一次同步訊號。""" + enemy_index = self.enemy_combo.findText(enemy_id) + a_index = self.a_group_combo.findData(a_group_id) + b_index = self.b_group_combo.findData(b_group_id) + if min(enemy_index, a_index, b_index) < 0: + return False + + self._suppress_enemy = True + self._suppress_group = True + try: + self.enemy_combo.setCurrentIndex(enemy_index) + self.a_group_combo.setCurrentIndex(a_index) + self.b_group_combo.setCurrentIndex(b_index) + self._enemy_id = enemy_id + finally: + self._suppress_enemy = False + self._suppress_group = False + + self.enemy_changed.emit(enemy_id) + self.groups_changed.emit(a_group_id, b_group_id) + return True + def set_phase_text(self, text): self.phase_label.setText(f"階段：{text}") @@ -650,36 +2069,45 @@ class EngagementPanel(QWidget): self.confirm_btn.setEnabled(enabled) def tick(self, drone_positions): - """{drone_id: (lat, lon)} 更新距離讀數。""" + """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 + + nearest_id = None + nearest_d = 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") + f"敵機 {enemy} ↔ 最近友機 {nearest_id}：{nearest_d:.1f} m" + ) self.distance_label.setStyleSheet( - f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;") + f"color: {color}; font-size: 13px; font-weight: bold; padding: 4px 2px;" + ) + + # ------------------------------------------------------------------ UI callbacks - # ----------------------------------------------------------------- 內部 def _on_enemy_changed(self, text): if self._suppress_enemy: return @@ -696,10 +2124,15 @@ class EngagementPanel(QWidget): 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") + 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})") + _log( + "INFO", + f"[交戰] 套用速度：友機 {v:.1f} m/s、敵機 {v * r:.1f} m/s (比例 {r:.2f})" + ) self.apply_speed_requested.emit(v, r) diff --git a/src/GUI/mission_orchestrator1.py b/src/GUI/mission_orchestrator1.py new file mode 100644 index 0000000..65f2e5f --- /dev/null +++ b/src/GUI/mission_orchestrator1.py @@ -0,0 +1,710 @@ +#!/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 = 25.0 # 接戰半徑 (m)：形心↔敵 ≤ 此值 → 切包圍 +DEFAULT_R_ENGAGE_EXIT = 35.0 # 包圍退出半徑 (m)：> 此值 → 退回前進（hysteresis） + +DEFAULT_REPLAN_MOVE_M = 2.0 # 敵機位移 ≥ 此值即重算（moving target） +DEFAULT_REPLAN_CAP_SEC = 1.0 # 重算週期上限 (s) + +DEFAULT_CIRCLE_RADIUS = 15.0 # 包圍半徑 (m) +DEFAULT_LON_SPACING = 8.0 # 隊形縱向間距 (m) +DEFAULT_LAT_OFFSET = 5.0 # 隊形橫向錯開 (m) + +DEFAULT_FORMATION_ALT = 30.0 # 隊形基準高度 (m，相對 home) +DEFAULT_ALT_LAYER_GAP = 10.0 # A/B 上下層垂直間隔 (m)：A=基準+gap/2、B=基準-gap/2 +DEFAULT_MIN_ALT_FLOOR = 10.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) diff --git a/src/GUI/mission_planner.py b/src/GUI/mission_planner.py index 9931f69..f4e5095 100644 --- a/src/GUI/mission_planner.py +++ b/src/GUI/mission_planner.py @@ -58,7 +58,7 @@ class FormationPlanner: """隊形規劃器""" def __init__(self, spacing: float = 5.0, - base_altitude: float = 25.0, + base_altitude: float = 30.0, altitude_diff: float = 2.0): self.spacing = spacing self.base_altitude = base_altitude @@ -179,7 +179,7 @@ class FormationPlanner: params = params or {} N = len(drone_positions) radius = params.get('radius', 10.0) - altitude = params.get('altitude', 25.0) + altitude = params.get('altitude', 30.0) start_angle = params.get('start_angle', 0.0) center_x, center_y, center_z = target_point