chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
master
wenchun
chiyu
chiyuTemp
lunu
ken
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'e93c73b9ec'
${ noResults }
AirTrapMine/src/GUI/mission_orchestrator.py

2193 lines
87 KiB
Python
Raw Blame History Unescape Escape

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

#!/usr/bin/env python3
"""
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
)
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)。"""
p1 = math.radians(lat1)
p2 = math.radians(lat2)
dphi = math.radians(lat2 - lat1)
dlam = math.radians(lon2 - lon1)
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) * 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 / 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
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 = 3.0
DEFAULT_ENEMY_VEL_WINDOW = 1.0
DEFAULT_TARGET_LOST_TIMEOUT = 5.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"
WATCH = "watch"
DETECTED = "detected"
CONVERGE = "converge" # 動態分配 slot + 發散/收斂曲線接近
ENCIRCLE = "encircle" # 所有友機已進入自己的 slot
TARGET_LOST = "target_lost" # 目標定位逾時，友機 hold
# ---------------------------------------------------------------------------
# Mission Orchestrator
class MissionOrchestrator(QObject):
"""
任務狀態機。
callback：
make_executor() -> MissionExecutor
gps_provider() -> {drone_id: {'lat','lon','alt'}}
stop_groups_fn() -> 操作員確認重編隊時，停止既有 A/B 群組任務
新策略：
WATCH -> DETECTED -> CONVERGE -> ENCIRCLE
進入 CONVERGE 後保留 A/B 分層：A/B 各自固定在敵機高度上下層；
同層必要時使用同心圓半徑車道避讓。跨層近距離時先垂直、後水平。
"""
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,
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.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.enemy_id = None
self._detect_since = 0.0
# 確認後的角色無關友機集合
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
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):
"""
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
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):
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 -> 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)
now = time.monotonic()
if not epos or not self._position_is_fresh(epos, now):
self.status_message.emit("敵機定位遺失，無法開始包圍")
return
if self._position_alt(epos) is None:
self.status_message.emit("敵機相對 Home 高度尚未收到，無法開始包圍")
return
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
if self._stop_groups_fn:
try:
self._stop_groups_fn()
except Exception as e:
_log("WARN", f"[orchestrator] 停止群組任務失敗: {e}")
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 = self._position_received_at(epos, now)
self._update_enemy_velocity(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()
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.CONVERGE)
if assignment:
self.status_message.emit("開始動態包圍：slot 鎖定，移動中同步展開隊形")
def stop(self):
"""停止任務並回 IDLE。"""
if self._merged_ex is not None:
try:
self._merged_ex.stop()
except Exception:
pass
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
def tick(self):
"""由 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():
self._detect_since = 0.0
self._set_phase(MissionPhase.IDLE)
if self.phase == MissionPhase.WATCH:
self._tick_watch()
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()
epos = gps.get(self.enemy_id)
now = time.monotonic()
if not epos or not self._position_is_fresh(epos, now):
self._detect_since = 0.0
return
nearest_id = None
nearest_d = None
for did in self.a_drone_ids:
pos = gps.get(did)
if not pos or not self._position_is_fresh(pos, now):
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
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)
self.detection_prompt.emit(
f"偵測到敵機（{nearest_id} 距 {nearest_d:.1f} m）— 請確認動態包圍"
)
else:
self._detect_since = 0.0
def _tick_converge(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
now = time.monotonic()
if (not epos or not self._position_is_fresh(epos, now)
or self._position_alt(epos) is None):
self._handle_enemy_missing(gps, now)
return
self._last_enemy_seen_at = self._position_received_at(epos, now)
self._update_enemy_velocity(epos)
if self._positions_for(self._friendly_ids, gps) is None:
return
if self._all_in_slots(gps, epos):
self._set_phase(MissionPhase.ENCIRCLE)
self.status_message.emit("所有友機已進入各自包圍 slot")
# 進 ENCIRCLE 當下仍送一次精確 slot
self._update_encircle_command(gps, epos, force=True)
return
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 or not self._position_is_fresh(epos, now)
or self._position_alt(epos) is None):
self._handle_enemy_missing(gps, now)
return
self._last_enemy_seen_at = self._position_received_at(epos, now)
self._update_enemy_velocity(epos)
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)
now = time.monotonic()
if (not epos or not self._position_is_fresh(epos, now)
or self._position_alt(epos) is None):
return
# 目標重新出現：不沿用過期軌跡，重新估速並回 CONVERGE。
self._enemy_hist = []
self._enemy_vel = (0.0, 0.0)
self._last_enemy_seen_at = self._position_received_at(epos, now)
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
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 = {}
now = time.monotonic()
for did in self._friendly_ids:
p = gps.get(did)
if not p or not self._position_is_fresh(p, now):
continue
current_alt = self._position_alt(p)
if current_alt is None:
continue
targets[did] = (
p['lat'],
p['lon'],
max(self.min_alt_floor, current_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
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):
"""更新敵機高度/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
):
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)
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):
return math.hypot(*self._enemy_vel)
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
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
try:
alt = float(pos.get('alt'))
except (TypeError, ValueError):
return None
return alt if math.isfinite(alt) else None
def _position_is_fresh(self, pos, now=None):
"""檢查 GPS 水平位置是否仍在可接受的遙測時間內。"""
if not pos:
return False
updated_at = pos.get('_gps_updated_at')
# 保留舊 gps_provider/測試資料的相容性；GUI 實飛資料一定帶時間戳。
if updated_at is None:
return True
try:
updated_at = float(updated_at)
except (TypeError, ValueError):
return False
if not math.isfinite(updated_at):
return False
now = time.monotonic() if now is None else float(now)
age = now - updated_at
return -0.1 <= age <= self.target_lost_timeout_sec
@staticmethod
def _position_received_at(pos, now):
"""取得最後一筆水平定位的 monotonic 時間，舊資料則使用目前時間。"""
try:
updated_at = float(pos.get('_gps_updated_at'))
except (AttributeError, TypeError, ValueError):
return now
if not math.isfinite(updated_at) or updated_at > now + 0.1:
return now
return updated_at
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 = {}
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
return slots
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))
return {
'p0': points[0],
'p3': points[-1],
'points': points,
'length': _polyline_length(points),
}
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
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 = self._position_alt(gps[did])
if cur_alt is None:
return None
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 = self._position_alt(p)
if cur_alt is None:
return None
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
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)
current_alt = self._position_alt(gps[did])
if current_alt is None:
return None
alt = max(self.min_alt_floor, current_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 _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)
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()
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
)
# ------------------------------------------------------------------ helpers
def _positions_for(self, drone_ids, gps):
out = []
now = time.monotonic()
for did in drone_ids:
p = gps.get(did)
if not p or not self._position_is_fresh(p, now):
return None
try:
lat = float(p['lat'])
lon = float(p['lon'])
except (KeyError, TypeError, ValueError):
return None
alt = self._position_alt(p)
if not math.isfinite(lat) or not math.isfinite(lon) or alt is None:
return None
out.append((lat, lon, alt))
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}")
self.phase_changed.emit(phase.value)
# ---------------------------------------------------------------------------
# Engagement Panel
class EngagementPanel(QWidget):
"""
交戰控制面板。
- 敵機下拉 + 即時敵機到最近友機距離
- A/B 僅用於偵察前的群組指定
- 偵測確認後由 MissionOrchestrator 合併為無角色 friendly 集合
- 速度比例 UI 保留，方便既有 GUI 發 DO_CHANGE_SPEED
"""
enemy_changed = pyqtSignal(str)
confirm_requested = pyqtSignal()
stop_requested = pyqtSignal()
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):
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
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)
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; }"
)
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)
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)]。"""
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_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}")
def set_confirm_enabled(self, enabled):
self.confirm_btn.setEnabled(enabled)
def tick(self, drone_positions):
"""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 = 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"
)
self.distance_label.setStyleSheet(
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
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)
Reference in New Issue View Git Blame Copy Permalink