chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2.9.1: new mission_orch

Browse Source
This commit is contained in:
ken910606
2026-09-16 01:32:09 +08:00
parent a345436008
commit e93c73b9ec
3 changed files with 180 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/communication.py
+69 -12
View File
@@ -11,6 +11,7 @@ import json
import socket
import sys
import os
import time
import traceback
try:
import serial
@@ -214,23 +215,23 @@ class JsonTelemetryProcessor:
pos = data.get('position')
if isinstance(pos, dict):
gps_data = {
'lat': pos.get('lat', pos.get('latitude', 0)),
'lon': pos.get('lon', pos.get('longitude', 0)),
'alt': pos.get('alt', pos.get('altitude', 0))
'lat': pos.get('lat', pos.get('latitude')),
'lon': pos.get('lon', pos.get('longitude')),
'alt': pos.get('alt', pos.get('altitude'))
}
self.signals.update_signal.emit('gps', drone_id, gps_data)
elif 'lat' in data or 'latitude' in data:
self.signals.update_signal.emit('gps', drone_id, {
'lat': data.get('lat', data.get('latitude', 0)),
'lon': data.get('lon', data.get('longitude', 0)),
'alt': data.get('alt', data.get('altitude', 0))
'lat': data.get('lat', data.get('latitude')),
'lon': data.get('lon', data.get('longitude')),
'alt': data.get('alt', data.get('altitude'))
})
elif isinstance(data.get('p'), (list, tuple)) and len(data.get('p')) >= 2:
dop = data.get('d') if isinstance(data.get('d'), (list, tuple)) else []
gps_data = {
'lat': data['p'][0],
'lon': data['p'][1],
'alt': data.get('h', 0)
'alt': data.get('h')
}
if 'g' in data:
gps_data['fix_type'] = data.get('g')
@@ -935,7 +936,6 @@ class DroneMonitor(Node):
def __init__(self):
# Use a unique node name with timestamp to avoid conflicts on restart
import time
node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
super().__init__(node_name)
self.signals = DroneSignals()
@@ -960,6 +960,9 @@ class DroneMonitor(Node):
# 【新增】儲存 GPS 資料的字典
# ================================================================================
self.drone_gps = {} # {drone_id: {'lat': ..., 'lon': ..., 'alt': ...}}
# GnssRaw.altitude 是 AMSL；交戰/航點指令的 alt 則是相對 Home 高度。
self.drone_relative_alt = {}
self.drone_relative_alt_updated_at = {}
# ================================================================================
# ================================================================================
@@ -1007,6 +1010,35 @@ class DroneMonitor(Node):
200
)
def get_drone_gps_snapshot(self):
"""回傳執行緒安全的定位快照。"""
with self.lock:
return {
drone_id: dict(position)
for drone_id, position in self.drone_gps.items()
if isinstance(position, dict)
}
def record_relative_gps(self, drone_id, gps_data, updated_at=None):
"""記錄已知以 Home 為基準的 GPS/高度（UDP、Serial、WebSocket）。"""
now = time.monotonic() if updated_at is None else float(updated_at)
stored = dict(gps_data)
stored['_gps_updated_at'] = now
alt = stored.get('alt')
if isinstance(alt, (int, float)) and math.isfinite(float(alt)):
stored['alt'] = float(alt)
stored['_relative_alt_updated_at'] = now
stored['_altitude_reference'] = 'relative_home'
else:
stored['alt'] = None
with self.lock:
if stored.get('_altitude_reference') == 'relative_home':
self.drone_relative_alt[drone_id] = stored['alt']
self.drone_relative_alt_updated_at[drone_id] = now
self.drone_gps[drone_id] = stored
return stored
def get_next_socket_id(self):
"""取得目前最小的未使用 socket_id（從 0 開始）。"""
with self.socket_id_lock:
@@ -1740,10 +1772,15 @@ class DroneMonitor(Node):
if actual_drone_id is None:
return
# GnssRaw.altitude 是 GLOBAL_POSITION_INT.alt（AMSL）。交戰航點使用
# GLOBAL_RELATIVE_ALT_INT，因此 alt 僅能使用 position_ned 的相對高度。
now = time.monotonic()
gps_data = {
'lat': msg.latitude,
'lon': msg.longitude,
'alt': msg.altitude
'alt': None,
'amsl_alt': msg.altitude,
'_gps_updated_at': now,
}
if hasattr(msg, 'fix_type'):
@@ -1755,12 +1792,20 @@ class DroneMonitor(Node):
if hasattr(msg, 'epv'):
gps_data['epv'] = msg.epv
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================
# 【新增】儲存 GPS 資料到 drone_gps 字典
# ================================================================================
self.drone_gps[actual_drone_id] = gps_data.copy()
with self.lock:
relative_alt = self.drone_relative_alt.get(actual_drone_id)
relative_alt_updated_at = self.drone_relative_alt_updated_at.get(
actual_drone_id
)
if relative_alt_updated_at is not None:
gps_data['alt'] = relative_alt
gps_data['_relative_alt_updated_at'] = relative_alt_updated_at
gps_data['_altitude_reference'] = 'relative_home'
self.drone_gps[actual_drone_id] = gps_data.copy()
self.latest_data[(actual_drone_id, 'gps')] = gps_data
# ================================================================================
def local_vel_callback(self, drone_id, msg):
@@ -1821,6 +1866,18 @@ class DroneMonitor(Node):
vy = msg.twist.twist.linear.x
vz = -msg.twist.twist.linear.z
now = time.monotonic()
with self.lock:
self.drone_relative_alt[actual_drone_id] = z
self.drone_relative_alt_updated_at[actual_drone_id] = now
current_gps = self.drone_gps.get(actual_drone_id)
if isinstance(current_gps, dict):
current_gps = dict(current_gps)
current_gps['alt'] = z
current_gps['_relative_alt_updated_at'] = now
current_gps['_altitude_reference'] = 'relative_home'
self.drone_gps[actual_drone_id] = current_gps
# 儲存高度信息
self.latest_data[(actual_drone_id, 'altitude')] = {
'altitude': z
src/GUI/gui.py
+34 -13
View File
@@ -147,10 +147,11 @@ class ToggleSwitch(QWidget):
class ControlStationUI(QMainWindow):
planning_finished = pyqtSignal(object)
VERSION = '2.9.0'
VERSION = '2.9.1'
FONT_SCALE_MIN = 70
FONT_SCALE_MAX = 180
FONT_SCALE_DEFAULT = 100
ENGAGEMENT_TELEMETRY_TIMEOUT_SEC = 5.0
def __init__(self):
super().__init__()
@@ -2647,13 +2648,27 @@ class ControlStationUI(QMainWindow):
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)
}
if hasattr(self.monitor, 'get_drone_gps_snapshot'):
snapshot = self.monitor.get_drone_gps_snapshot()
else:
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 position in snapshot.values():
relative_updated_at = position.get('_relative_alt_updated_at')
relative_is_fresh = (
position.get('_altitude_reference') == 'relative_home'
and isinstance(relative_updated_at, (int, float))
and 0.0 <= now - relative_updated_at
<= self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC
)
if not relative_is_fresh:
# 不得把 AMSL 或過期高度帶入 GLOBAL_RELATIVE_ALT 指令。
position.pop('alt', None)
for drone_id, heading in self.drone_headings.items():
updated_at = self.drone_heading_updated_at.get(drone_id)
heading_is_fresh = (
@@ -2672,6 +2687,7 @@ class ControlStationUI(QMainWindow):
make_executor=self._make_orchestrator_executor,
gps_provider=self._engagement_gps_provider,
stop_groups_fn=self._stop_engagement_group_missions,
target_lost_timeout_sec=self.ENGAGEMENT_TELEMETRY_TIMEOUT_SEC,
)
self._engagement_a_group = None
self._engagement_b_group = None
@@ -2832,8 +2848,8 @@ class ControlStationUI(QMainWindow):
assignments = {
'A': {drones_by_sysid[11], drones_by_sysid[12]},
'B': {drones_by_sysid[14], drones_by_sysid[15]},
'C': {drones_by_sysid[13]},
'B': {drones_by_sysid[13], drones_by_sysid[14]},
'C': {drones_by_sysid[15]},
}
for gid, members in assignments.items():
group = self.mission_groups[gid]
@@ -2853,13 +2869,13 @@ class ControlStationUI(QMainWindow):
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]
enemy_id = drones_by_sysid[15]
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）",
"已自動設定交戰：A=sysid 11,12；B=sysid 13,14；敵機=sysid 15（Group C）",
6000)
def reorganize_socket_groups(self):
@@ -3021,7 +3037,7 @@ class ControlStationUI(QMainWindow):
)
elif msg_type == 'gps':
gps_data = data
gps_data = dict(data)
lat, lon = gps_data.get('lat'), gps_data.get('lon')
has_position = (
isinstance(lat, (int, float)) and isinstance(lon, (int, float))
@@ -3031,7 +3047,12 @@ class ControlStationUI(QMainWindow):
self._map_dirty_drones.add(drone_id)
if not hasattr(self.monitor, 'drone_gps'):
self.monitor.drone_gps = {}
self.monitor.drone_gps[drone_id] = gps_data.copy()
# ROS callback 已先寫入含時間戳的快照；其餘來源的
# GLOBAL_POSITION_INT/JSON 高度均定義為相對 Home。
if '_gps_updated_at' not in gps_data:
gps_data = self.monitor.record_relative_gps(
drone_id, gps_data
)
self.queue_overview_update(
drone_id, 'latitude', f"{lat:.6f}°" if has_position else '-'
)
src/GUI/mission_orchestrator.py
+77 -23
View File
@@ -182,9 +182,9 @@ 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_FRIENDLY_SPEED = 3.0
DEFAULT_ENEMY_VEL_WINDOW = 1.0
DEFAULT_TARGET_LOST_TIMEOUT = 2.0
DEFAULT_TARGET_LOST_TIMEOUT = 5.0
# assignment cost
DEFAULT_CROSSING_PENALTY = 20000.0
@@ -437,9 +437,13 @@ class MissionOrchestrator(QObject):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
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:
@@ -467,7 +471,7 @@ class MissionOrchestrator(QObject):
self._motion_dirs = {}
self._slot_groups = {} # {slot_index: "A"/"B"}
self._vertical_hold_ids = set()
self._last_enemy_seen_at = time.monotonic()
self._last_enemy_seen_at = self._position_received_at(epos, now)
self._update_enemy_velocity(epos)
center = self._planning_center(gps, epos, reset=True)
@@ -558,7 +562,8 @@ class MissionOrchestrator(QObject):
def _tick_watch(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
now = time.monotonic()
if not epos or not self._position_is_fresh(epos, now):
self._detect_since = 0.0
return
@@ -566,13 +571,12 @@ class MissionOrchestrator(QObject):
nearest_d = None
for did in self.a_drone_ids:
pos = gps.get(did)
if not pos:
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
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
@@ -589,11 +593,12 @@ class MissionOrchestrator(QObject):
epos = gps.get(self.enemy_id)
now = time.monotonic()
if not epos:
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 = now
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:
@@ -614,11 +619,12 @@ class MissionOrchestrator(QObject):
epos = gps.get(self.enemy_id)
now = time.monotonic()
if not epos:
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 = now
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:
@@ -638,13 +644,15 @@ class MissionOrchestrator(QObject):
def _tick_target_lost(self):
gps = self._gps_provider()
epos = gps.get(self.enemy_id)
if not epos:
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 = time.monotonic()
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("敵機定位恢復，重新建立包圍軌跡")
@@ -667,14 +675,18 @@ class MissionOrchestrator(QObject):
if self._merged_ex is None:
return
targets = {}
now = time.monotonic()
for did in self._friendly_ids:
p = gps.get(did)
if not p:
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, float(p.get('alt', self.formation_alt))),
max(self.min_alt_floor, current_alt),
)
if len(targets) == len(self._friendly_ids):
self._merged_ex.update_plan(self._targets_to_plan(targets))
@@ -844,6 +856,35 @@ class MissionOrchestrator(QObject):
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 友機。
@@ -1111,7 +1152,9 @@ class MissionOrchestrator(QObject):
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))
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']))
@@ -1317,7 +1360,9 @@ class MissionOrchestrator(QObject):
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))
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))
@@ -1360,10 +1405,10 @@ class MissionOrchestrator(QObject):
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)),
)
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
@@ -1776,11 +1821,20 @@ class MissionOrchestrator(QObject):
def _positions_for(self, drone_ids, gps):
out = []
now = time.monotonic()
for did in drone_ids:
p = gps.get(did)
if not p:
if not p or not self._position_is_fresh(p, now):
return None
out.append((p['lat'], p['lon'], p.get('alt', 0.0)))
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