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

Compare commits

base: chiyu1468:master
Branches Tags
chiyu1468:master
chiyu1468:wenchun
chiyu1468:lunu
chiyu1468:chiyu
chiyu1468:ken
..
compare: chiyu1468:wenchun
Branches Tags
chiyu1468:master
chiyu1468:wenchun
chiyu1468:lunu
chiyu1468:chiyu
chiyu1468:ken

No commits in common. 'master' and 'wenchun' have entirely different histories.
master ... wenchun

2 changed files with 268 additions and 575 deletions
Show Stats

338
src/GUI/communication.py
Unescape Escape View File

@ -12,10 +12,6 @@ import socket
import sys
import os
import traceback
try:
import serial
except ImportError:
serial = None
from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3, Vector3Stamped, PoseWithCovarianceStamped
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@ -65,153 +61,6 @@ except ImportError as e:
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
class JsonTelemetryProcessor:
"""共用 WebSocket JSON telemetry 轉換器。
Canonical JSON fields:
{
"system_id": 1,
"mode": "GUIDED",
"battery": 85,
"position": {"lat": 24.0, "lon": 120.0},
"heading": 90
}
Serial JSON uses this same shape; only the transport/framing differs.
"""
def _emit_json_connection_type(self, drone_id):
self.signals.update_signal.emit('connection_type', drone_id, {
'type': self.source_type
})
def process_json_telemetry_message(self, data):
"""處理 WebSocket JSON 格式的遙測資料。"""
try:
if isinstance(data, list):
for item in data:
if isinstance(item, dict):
self.process_json_telemetry_message(item)
return
if not isinstance(data, dict):
return
system_id = data.get('system_id', data.get('sysid'))
if system_id is None:
return
drone_id = f"s{self.socket_id}_{system_id}"
self._emit_json_connection_type(drone_id)
mode = data.get('mode', data.get('mode_name'))
state = {}
if mode is not None:
state['mode'] = mode
if 'armed' in data:
state['armed'] = data.get('armed')
if state:
self.signals.update_signal.emit('state', drone_id, state)
if 'battery' in data:
battery = data['battery']
if isinstance(battery, dict):
battery_data = {}
if 'percentage' in battery:
battery_data['percentage'] = battery.get('percentage')
elif 'percent' in battery:
battery_data['percentage'] = battery.get('percent')
if 'voltage' in battery:
battery_data['voltage'] = battery.get('voltage')
elif 'voltage_v' in battery:
battery_data['voltage'] = battery.get('voltage_v')
if battery_data:
self.signals.update_signal.emit('battery', drone_id, battery_data)
else:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': battery
})
elif 'battery_percentage' in data or 'battery_voltage' in data:
battery_data = {}
if 'battery_percentage' in data:
battery_data['percentage'] = data.get('battery_percentage')
if 'battery_voltage' in data:
battery_data['voltage'] = data.get('battery_voltage')
self.signals.update_signal.emit('battery', drone_id, battery_data)
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))
}
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))
})
local = data.get('local_position', data.get('local_pose', data.get('local')))
if isinstance(local, dict):
x = local.get('x', 0.0)
y = local.get('y', 0.0)
z = local.get('z', 0.0)
self.signals.update_signal.emit('local_pose', drone_id, {
'x': x,
'y': y,
'z': z
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': z
})
elif isinstance(pos, dict):
alt = pos.get('alt', pos.get('altitude', 0.0))
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': alt
})
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': alt
})
velocity = data.get('velocity')
if isinstance(velocity, dict):
self.signals.update_signal.emit('velocity', drone_id, {
'vx': velocity.get('vx', velocity.get('x', 0.0)),
'vy': velocity.get('vy', velocity.get('y', 0.0)),
'vz': velocity.get('vz', velocity.get('z', 0.0))
})
attitude = data.get('attitude')
if isinstance(attitude, dict):
self.signals.update_signal.emit('attitude', drone_id, {
'roll': attitude.get('roll', 0.0),
'pitch': attitude.get('pitch', 0.0),
'yaw': attitude.get('yaw', 0.0),
'rates': attitude.get('rates', (0.0, 0.0, 0.0))
})
hud = data.get('hud', {})
if not isinstance(hud, dict):
hud = {}
if 'heading' in data:
hud['heading'] = data.get('heading')
if hud:
self.signals.update_signal.emit('hud', drone_id, {
'heading': hud.get('heading', 0.0),
'groundspeed': hud.get('groundspeed', 0.0),
'airspeed': hud.get('airspeed', 0.0),
'throttle': hud.get('throttle', 0.0),
'alt': hud.get('alt', hud.get('altitude', 0.0)),
'climb': hud.get('climb', 0.0)
})
except Exception as e:
print(f"{self.source_type} JSON telemetry processing error: {e}")
class UDPMavlinkReceiver(threading.Thread):
"""UDP MAVLink 接收器"""
def __init__(self, ip, port, signals, connection_name, monitor=None):
@ -335,8 +184,8 @@ class UDPMavlinkReceiver(threading.Thread):
"""停止接收器"""
self.running = False
class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
"""串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。"""
class SerialMavlinkReceiver(threading.Thread):
"""串口 MAVLink 接收器"""
def __init__(self, port, baudrate, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.port = port
@ -345,125 +194,47 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
self.socket_id = monitor.get_next_socket_id() if monitor else 0
self.source_type = 'Serial'
self.running = False
self.serial_conn = None
self.mav_parser = mavutil.mavlink.MAVLink(None)
self.mav_parser.robust_parsing = True
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
self._detected_protocols = set()
self.mav = None
def run(self):
"""執行串口接收循環。"""
"""執行串口接收循環"""
self.running = True
try:
print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)")
if serial is None:
raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud")
self.serial_conn = serial.Serial(
# 創建 MAVLink 串口連接
self.mav = mavutil.mavlink_connection(
self.port,
self.baudrate,
timeout=0.2
baud=self.baudrate,
source_system=255
)
print(f"Waiting for heartbeat from {self.port}...")
self.mav.wait_heartbeat()
print(f"Heartbeat received from system {self.mav.target_system}, component {self.mav.target_component}")
while self.running:
try:
chunk = self.serial_conn.read(256)
if not chunk:
msg = self.mav.recv_match(blocking=True, timeout=1.0)
if msg is None:
continue
for raw_byte in chunk:
byte = bytes([raw_byte])
self._process_json_byte(byte)
self._process_mavlink_byte(byte)
self.process_mavlink_message(msg)
except Exception as e:
if self.running:
print(f"Error receiving serial telemetry: {e}")
print(f"Error receiving MAVLink message from serial: {e}")
except Exception as e:
print(f"Serial receiver error: {e}")
finally:
if self.serial_conn:
if self.mav:
try:
self.serial_conn.close()
except Exception:
self.mav.close()
except:
pass
def _process_mavlink_byte(self, byte):
try:
msg = self.mav_parser.parse_char(byte)
if msg is None:
return
if 'MAVLink' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected MAVLink")
self._detected_protocols.add('MAVLink')
self.process_mavlink_message(msg)
except Exception:
# MAVLink parser is deliberately fed the whole stream, including JSON bytes.
return
def _process_json_byte(self, byte):
try:
char = byte.decode('utf-8')
except UnicodeDecodeError:
if self.json_buffer:
self._reset_json_framing()
return
if not self.json_buffer:
if char.isspace():
return
if char not in ('{', '['):
return
self.json_depth = 1
self.json_in_string = False
self.json_escape = False
self.json_buffer = [char]
return
self.json_buffer.append(char)
if self.json_in_string:
if self.json_escape:
self.json_escape = False
elif char == '\\':
self.json_escape = True
elif char == '"':
self.json_in_string = False
return
if char == '"':
self.json_in_string = True
elif char in ('{', '['):
self.json_depth += 1
elif char in ('}', ']'):
self.json_depth -= 1
if self.json_depth > 0:
return
payload = ''.join(self.json_buffer)
self._reset_json_framing()
try:
data = json.loads(payload)
if 'JSON' not in self._detected_protocols:
print(f"Serial {self.connection_name} detected JSON")
self._detected_protocols.add('JSON')
self.process_json_telemetry_message(data)
except json.JSONDecodeError as e:
print(f"Serial {self.connection_name} JSON decode error: {e}")
def _reset_json_framing(self):
self.json_buffer = []
self.json_depth = 0
self.json_in_string = False
self.json_escape = False
def process_mavlink_message(self, msg):
"""處理 MAVLink 訊息"""
try:
@ -546,23 +317,15 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def stop(self):
"""停止接收器"""
self.running = False
if self.serial_conn:
try:
self.serial_conn.close()
except Exception:
pass
class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
class WebSocketMavlinkReceiver(threading.Thread):
"""WebSocket MAVLink 接收器"""
def __init__(self, url, signals, connection_name, monitor=None):
super().__init__(daemon=True)
self.url = url
self.signals = signals
self.connection_name = connection_name
self.monitor = monitor # 保存 monitor 引用
self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id
self.source_type = 'WS'
self.running = False
self.monitor = monitor # 保存 monitor 引用 self.socket_id = monitor.get_next_socket_id() if monitor else 0 # 一次性分配 socket_id self.running = False
self.max_retries = 5
self.base_delay = 1.0
@ -590,7 +353,6 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
try:
data = json.loads(message)
if isinstance(data, dict):
data['_connection_source'] = self.connection_name
self.process_websocket_message(data)
except json.JSONDecodeError as e:
@ -621,7 +383,57 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor):
def process_websocket_message(self, data):
"""處理 WebSocket 訊息"""
self.process_json_telemetry_message(data)
try:
system_id = data.get('system_id')
if not system_id:
return
drone_id = f"s{self.socket_id}_{system_id}"
# 模式
if 'mode' in data:
# 先發送連接類型資訊
self.signals.update_signal.emit('connection_type', drone_id, {
'type': 'WS'
})
self.signals.update_signal.emit('state', drone_id, {
'mode': data['mode'],
})
# 電池
if 'battery' in data:
self.signals.update_signal.emit('battery', drone_id, {
'percentage': data['battery']
})
# 位置
if 'position' in data:
pos = data['position']
self.signals.update_signal.emit('gps', drone_id, {
'lat': pos.get('lat', 0),
'lon': pos.get('lon', 0)
})
# Local position - 設定 x, y 為 0.0
self.signals.update_signal.emit('local_pose', drone_id, {
'x': 0.0,
'y': 0.0,
'z': 0.0
})
# Altitude - 設定為 0.0
self.signals.update_signal.emit('altitude', drone_id, {
'altitude': 0.0
})
# 航向
if 'heading' in data:
self.signals.update_signal.emit('hud', drone_id, {
'heading': data['heading'],
'groundspeed': 0.0
})
except Exception as e:
print(f"WebSocket message processing error: {e}")
def stop(self):
"""停止接收器"""
@ -1289,10 +1101,10 @@ class DroneMonitor(Node):
}
def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600):
"""啟動串口遙測連接（自動辨識 MAVLink / JSON）"""
"""啟動串口 MAVLink 連接"""
connection_name = f"Serial_{port.replace('/', '_')}"
receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self)
receiver.start()
self.serial_receivers.append(receiver)
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)")
_log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud")
return receiver

427
src/GUI/gui.py
Unescape Escape View File

@ -14,8 +14,6 @@ import subprocess
import time
import traceback
import re
import threading
from concurrent.futures import ThreadPoolExecutor
def _log(level, message):
@ -145,9 +143,7 @@ class ToggleSwitch(QWidget):
class ControlStationUI(QMainWindow):
planning_finished = pyqtSignal(object)
VERSION = '2.3.0'
VERSION = '2.2.0'
FONT_SCALE_MIN = 70
FONT_SCALE_MAX = 180
FONT_SCALE_DEFAULT = 100
@ -156,7 +152,6 @@ class ControlStationUI(QMainWindow):
super().__init__()
self.setWindowTitle(f'GCS v{self.VERSION}')
self.resize(1400, 900)
self._closing = False
# 初始化消息隊列（用於線程安全的 GUI 更新）
import queue
@ -173,16 +168,10 @@ class ControlStationUI(QMainWindow):
# 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
self.monitor.executor = self.executor
# 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間
self.ros_thread_running = True
self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True)
self.ros_thread.start()
# 任務規劃移到 worker thread，避免大量航點計算卡住 Qt 主執行緒
self.planner_pool = ThreadPoolExecutor(max_workers=2)
self._plan_request_counter = 0
self._latest_plan_request_by_group = {}
self.planning_finished.connect(self._on_plan_done)
# 定时处理ROS事件
self.timer = QTimer()
self.timer.timeout.connect(self.spin_ros)
self.timer.start(10)
# 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
# 這允許異步任務（如 arm_drone）能夠執行
@ -190,7 +179,7 @@ class ControlStationUI(QMainWindow):
self.asyncio_timer.timeout.connect(self._spin_asyncio)
self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio
# 初始化 panel 更新（10Hz）
# 初始化 panel 和 map 更新（10Hz）
self.panel_map_timer = QTimer()
self.panel_map_timer.timeout.connect(self._update_panel_and_map)
self.panel_map_timer.start(100) # 10Hz
@ -202,7 +191,6 @@ class ControlStationUI(QMainWindow):
# 快取消息數據，以便在沒有新消息時使用上一次的值
self._message_cache = {}
self._overview_cache = {}
self.message_history = []
self.max_message_history = 500
@ -229,17 +217,6 @@ class ControlStationUI(QMainWindow):
self.drone_headings = {}
# 初始化地圖
self.drone_map = DroneMap()
# 地圖更新獨立降頻（5Hz），避免 WebEngine / JS map 拖慢 panel 更新
self.map_timer = QTimer()
self.map_timer.timeout.connect(self._update_map_only)
self.map_timer.start(200)
# Overview table 獨立批次更新（5Hz），避免每筆資料都重繪 Qt table
self.overview_timer = QTimer()
self.overview_timer.timeout.connect(self._flush_overview_table)
self.overview_timer.start(200)
# 初始化連接列表
self.udp_receivers = []
self.udp_connections = []
@ -749,7 +726,7 @@ class ControlStationUI(QMainWindow):
self.message_history = self.message_history[-self.max_message_history:]
if hasattr(self, 'message_history_view'):
self.message_history_view.appendPlainText(entry)
self.message_history_view.setPlainText("\n".join(self.message_history))
scrollbar = self.message_history_view.verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())
@ -823,7 +800,7 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("已停止")
self.statusBar().showMessage(f"已停止 WebSocket 連接: {conn['url']}", 3000)
else:
receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'], self.monitor)
receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'])
receiver.start()
self.monitor.ws_receivers.append(receiver)
conn['receiver'] = receiver
@ -855,7 +832,7 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("已停止")
self.statusBar().showMessage(f"已停止 UDP 連接: {conn['ip']}:{conn['port']}", 3000)
else:
receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'], self.monitor)
receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'])
receiver.start()
self.udp_receivers.append(receiver)
conn['receiver'] = receiver
@ -1756,141 +1733,6 @@ class ControlStationUI(QMainWindow):
"""取得群組的無人機 ID 列表（排序後）"""
return sorted(group.selected_drone_ids, key=lambda x: (x.split('_')[0], int(x.split('_')[1])))
@staticmethod
def _run_plan_formation_mission(planner_config, drone_gps_positions,
target_gps, mission_type, params):
planner = FormationPlanner(**planner_config)
return planner.plan_formation_mission(
drone_gps_positions, target_gps, mission_type, params=params
)
def _submit_mission_plan(self, group, selected_drones, drone_gps_positions,
target_gps, mission_type, params, context):
self._plan_request_counter += 1
request_id = self._plan_request_counter
self._latest_plan_request_by_group[group.group_id] = request_id
planner_config = {
'spacing': self.mission_planner.spacing,
'base_altitude': self.mission_planner.base_altitude,
'altitude_diff': self.mission_planner.altitude_diff,
}
params = dict(params)
future = self.planner_pool.submit(
self._run_plan_formation_mission,
planner_config,
list(drone_gps_positions),
tuple(target_gps),
mission_type,
params,
)
future.add_done_callback(
lambda f: self._emit_plan_result(
f, request_id, group.group_id, list(selected_drones),
list(drone_gps_positions), tuple(target_gps), context
)
)
def _emit_plan_result(self, future, request_id, group_id, selected_drones,
drone_gps_positions, target_gps, context):
if getattr(self, '_closing', False):
return
try:
waypoints_per_drone, center_origin, rendezvous_indices = future.result()
payload = {
'ok': True,
'request_id': request_id,
'group_id': group_id,
'selected_drones': selected_drones,
'drone_gps_positions': drone_gps_positions,
'target_gps': target_gps,
'waypoints_per_drone': waypoints_per_drone,
'center_origin': center_origin,
'rendezvous_indices': rendezvous_indices,
'context': context,
}
except Exception as e:
payload = {
'ok': False,
'request_id': request_id,
'group_id': group_id,
'error': str(e),
'traceback': traceback.format_exc(),
'context': context,
}
self.planning_finished.emit(payload)
def _on_plan_done(self, payload):
if getattr(self, '_closing', False):
return
group_id = payload['group_id']
context = payload.get('context', {})
if self._latest_plan_request_by_group.get(group_id) != payload['request_id']:
return
group = self.mission_groups.get(group_id)
if not group:
return
if not payload['ok']:
self.statusBar().showMessage(
f"Group {group_id}: {context.get('failure_label', '規劃')}失敗: {payload['error']}", 5000
)
_log("ERROR", payload.get('traceback', ''))
return
selected_drones = payload['selected_drones']
drone_gps_positions = payload['drone_gps_positions']
target_gps = payload['target_gps']
waypoints_per_drone = payload['waypoints_per_drone']
center_origin = payload['center_origin']
rendezvous_indices = payload['rendezvous_indices']
expected_mission_type = context.get('expected_mission_type')
if set(group.selected_drone_ids) != set(selected_drones):
self.statusBar().showMessage(
f"Group {group_id}: 無人機分配已變更，忽略舊規劃結果", 3000
)
return
if expected_mission_type and group.mission_type != expected_mission_type:
self.statusBar().showMessage(
f"Group {group_id}: 任務模式已變更，忽略舊規劃結果", 3000
)
return
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
group.center_origin = center_origin
self.show_planned_waypoints(group)
center_lat, center_lon, _ = center_origin
target_lat, target_lon = context['draw_target']
self.drone_map.draw_mission_plan_for_group(
group_id, group.color,
center_lat, center_lon, target_lat, target_lon
)
self._launch_verification(
context['verification_type'], drone_gps_positions, selected_drones,
waypoints_per_drone, center_origin,
target_gps=target_gps if context.get('use_target_gps') else None,
rect_corners=context.get('rect_corners'),
route_waypoints=context.get('route_waypoints'),
)
panel = self.group_panels.get(group_id)
if panel:
panel.update_status()
panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"Group {group_id}: {context['success_label']}，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
def handle_map_click(self, lat, lon):
"""處理地圖點擊事件 — 根據 active group 的任務類型規劃"""
group = self._get_active_group()
@ -1919,23 +1761,47 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000)
try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
if drone_gps_positions is None:
return
context = {
'success_label': f"{group.mission_type} 規劃完成",
'failure_label': '規劃',
'verification_type': group.mission_type,
'draw_target': (lat, lon),
'use_target_gps': True,
'expected_mission_type': group.mission_type,
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, mission_type, params=params
)
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
self._submit_mission_plan(
group, selected_drones, drone_gps_positions,
target_gps, mission_type, params, context
group.center_origin = center_origin
self.show_planned_waypoints(group)
center_lat, center_lon, _ = center_origin
self.drone_map.draw_mission_plan_for_group(
group.group_id, group.color,
center_lat, center_lon, lat, lon
)
self._launch_verification(
group.mission_type, drone_gps_positions, selected_drones,
waypoints_per_drone, center_origin, target_gps=target_gps
)
panel = self.group_panels.get(group.group_id)
if panel:
panel.update_status()
panel.update_mission_info(center_lat, center_lon, lat, lon)
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"Group {group.group_id}: {group.mission_type} 規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
except Exception as e:
self.statusBar().showMessage(f"Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
# 任務規劃 — 矩形選取 (Grid Sweep)
# ================================================================================
@ -1965,6 +1831,7 @@ class ControlStationUI(QMainWindow):
base_alt = params.get('altitude', 10.0)
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000)
try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
if drone_gps_positions is None:
return
@ -1974,18 +1841,41 @@ class ControlStationUI(QMainWindow):
target_gps = (target_lat, target_lon, base_alt)
params['rect_corners'] = rect_corners
context = {
'success_label': 'Grid Sweep 規劃完成',
'failure_label': 'Grid Sweep 規劃',
'verification_type': 'grid_sweep',
'draw_target': (target_lat, target_lon),
'rect_corners': rect_corners,
'expected_mission_type': group.mission_type,
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.GRID_SWEEP,
params=params
)
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
self._submit_mission_plan(
group, selected_drones, drone_gps_positions,
target_gps, MissionType.GRID_SWEEP, params, context
group.center_origin = center_origin
self.show_planned_waypoints(group)
center_lat, center_lon, _ = center_origin
self.drone_map.draw_mission_plan_for_group(
group.group_id, group.color,
center_lat, center_lon, target_lat, target_lon
)
self._launch_verification(
'grid_sweep', drone_gps_positions, selected_drones,
waypoints_per_drone, center_origin, rect_corners=rect_corners
)
panel = self.group_panels.get(group.group_id)
if panel:
panel.update_status()
panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"Group {group.group_id}: Grid Sweep 規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
except Exception as e:
self.statusBar().showMessage(f"Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
# 任務規劃 — 路徑確認 (Leader-Follower)
@ -2027,6 +1917,7 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage(
f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000)
try:
drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt)
if drone_gps_positions is None:
return
@ -2036,19 +1927,43 @@ class ControlStationUI(QMainWindow):
target_gps = (target_lat, target_lon, base_alt)
params['route_waypoints'] = route_waypoints
context = {
'success_label': '跟隨模式規劃完成',
'failure_label': '跟隨模式規劃',
'verification_type': 'LEADER_FOLLOWER',
'draw_target': (target_lat, target_lon),
'use_target_gps': True,
'route_waypoints': route_waypoints,
'expected_mission_type': group.mission_type,
waypoints_per_drone, center_origin, rendezvous_indices = self.mission_planner.plan_formation_mission(
drone_gps_positions, target_gps, MissionType.LEADER_FOLLOWER,
params=params
)
group.planned_waypoints = {
'drone_ids': selected_drones,
'waypoints': waypoints_per_drone,
'rendezvous_indices': rendezvous_indices,
}
self._submit_mission_plan(
group, selected_drones, drone_gps_positions,
target_gps, MissionType.LEADER_FOLLOWER, params, context
group.center_origin = center_origin
self.show_planned_waypoints(group)
center_lat, center_lon, _ = center_origin
self.drone_map.draw_mission_plan_for_group(
group.group_id, group.color,
center_lat, center_lon, target_lat, target_lon
)
self._launch_verification(
'LEADER_FOLLOWER', drone_gps_positions, selected_drones,
waypoints_per_drone, center_origin,
target_gps=target_gps, route_waypoints=route_waypoints
)
panel = self.group_panels.get(group.group_id)
if panel:
panel.update_status()
panel.update_mission_info(center_lat, center_lon, target_lat, target_lon)
total_wps = sum(len(wps) for wps in waypoints_per_drone)
self.statusBar().showMessage(
f"Group {group.group_id}: 跟隨模式規劃完成，{len(selected_drones)} 台，共 {total_wps} 個航點", 5000
)
except Exception as e:
self.statusBar().showMessage(f"Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
traceback.print_exc()
# ================================================================================
# 任務執行回呼
@ -2149,30 +2064,6 @@ class ControlStationUI(QMainWindow):
self.overview_table.set_drones(self.drones)
self.overview_table.update_table(drone_id, field, value)
def queue_overview_update(self, drone_id, field, value):
if not hasattr(self, '_overview_cache'):
self._overview_cache = {}
self._overview_cache.setdefault(drone_id, {})[field] = value
def _flush_overview_table(self):
if not hasattr(self, 'overview_table') or self.overview_table is None:
return
if not getattr(self, '_overview_cache', None):
return
pending_updates = self._overview_cache
self._overview_cache = {}
self.overview_table.set_drones(self.drones)
self.overview_table.setUpdatesEnabled(False)
try:
for drone_id, fields in pending_updates.items():
for field, value in fields.items():
self.overview_table.update_table(drone_id, field, value)
finally:
self.overview_table.setUpdatesEnabled(True)
self.overview_table.viewport().update()
def get_socket_id(self, drone_id):
import re
match = re.match(r's(\d+)_(\d+)', drone_id)
@ -2209,7 +2100,7 @@ class ControlStationUI(QMainWindow):
self.drone_panel_layout.addWidget(self.socket_groups[socket_id])
def _update_panel_and_map(self):
"""定時更新 panel / table，批量更新 UI 以避免過度重繪。"""
"""30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪"""
if not hasattr(self, '_message_cache') or not self._message_cache:
return
@ -2224,6 +2115,10 @@ class ControlStationUI(QMainWindow):
self._map_update_time = now
self._map_update_count = 0
# ✅ 步驟 1: 暫停表格的即時重繪
if hasattr(self, 'overview_table') and self.overview_table:
self.overview_table.setUpdatesEnabled(False)
try:
start_time = time.time()
@ -2249,8 +2144,8 @@ class ControlStationUI(QMainWindow):
arm_text, arm_color = "--", '#AAAAAA'
self.update_field(panel, drone_id, 'mode', mode, mode_color)
self.update_field(panel, drone_id, 'armed', arm_text, arm_color)
self.queue_overview_update(drone_id, 'mode', mode)
self.queue_overview_update(drone_id, 'armed', arm_text)
self.update_overview_table(drone_id, 'mode', mode)
self.update_overview_table(drone_id, 'armed', arm_text)
elif msg_type == 'battery':
voltage = data.get('voltage', 16)
@ -2263,39 +2158,39 @@ class ControlStationUI(QMainWindow):
self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color)
self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V")
self.update_field(panel, drone_id, 'battery_cells', f"{cells}S")
self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V")
self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V")
elif msg_type == 'altitude':
altitude = data.get('altitude', 0)
text = f"{altitude:.1f} m"
self.update_field(panel, drone_id, 'altitude', text)
self.queue_overview_update(drone_id, 'altitude', text)
self.update_overview_table(drone_id, 'altitude', text)
elif msg_type == 'local_pose':
x, y = data.get('x', 0), data.get('y', 0)
if not hasattr(self.monitor, 'drone_local'):
self.monitor.drone_local = {}
self.monitor.drone_local[drone_id] = {'x': x, 'y': y}
self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}")
self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}")
elif msg_type == 'loss_rate':
text = f"{data.get('loss_rate', 0):.1f}%"
self.update_field(panel, drone_id, 'loss_rate', text)
self.queue_overview_update(drone_id, 'loss_rate', text)
self.update_overview_table(drone_id, 'loss_rate', text)
elif msg_type == 'ping':
text = f"{data.get('ping', 0):.1f} ms"
self.update_field(panel, drone_id, 'ping', text)
self.queue_overview_update(drone_id, 'ping', text)
self.update_overview_table(drone_id, 'ping', text)
elif msg_type == 'velocity':
self.queue_overview_update(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}")
elif msg_type == 'attitude':
roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0)
self.queue_overview_update(drone_id, 'roll', f"{roll:.1f}°")
self.queue_overview_update(drone_id, 'pitch', f"{pitch:.1f}°")
self.queue_overview_update(drone_id, 'yaw', f"{yaw:.1f}°")
self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°")
self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°")
self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°")
panel._last_roll = roll
panel._last_pitch = pitch
if hasattr(panel, 'update_attitude'):
@ -2310,8 +2205,8 @@ class ControlStationUI(QMainWindow):
if not hasattr(self.monitor, 'drone_gps'):
self.monitor.drone_gps = {}
self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt}
self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°")
self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°")
self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°")
self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°")
elif msg_type == 'hud':
hud_data = data
@ -2323,31 +2218,30 @@ class ControlStationUI(QMainWindow):
hud_alt = hud_data.get('alt', 0)
climb = hud_data.get('climb', 0)
self.queue_overview_update(drone_id, 'heading', f"{heading:.1f}°")
self.queue_overview_update(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°")
self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--")
self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--")
self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--")
self.update_overview_table(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--")
self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°")
self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--")
if drone_id in self.drone_positions:
lat, lon = self.drone_positions[drone_id]
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
elapsed = (time.time() - start_time) * 1000
if elapsed > 33:
_log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)")
except Exception as e:
_log("ERROR", f"Panel 更新錯誤: {e}")
traceback.print_exc()
def _update_map_only(self):
"""獨立降頻更新地圖，避免地圖 JS 呼叫拖住 panel / table。"""
for drone_id, pos in list(self.drone_positions.items()):
heading = self.drone_headings.get(drone_id, 0)
lat, lon = pos
self.drone_map.update_drone_position(drone_id, lat, lon, heading)
finally:
# ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
if hasattr(self, 'overview_table') and self.overview_table:
self.overview_table.setUpdatesEnabled(True)
self.overview_table.viewport().update()
@ -2381,41 +2275,28 @@ class ControlStationUI(QMainWindow):
# 但仍然打印詳細的堆棧跟踪以便調試
traceback.print_exc()
def _ros_spin_thread(self):
while self.ros_thread_running and rclpy.ok():
def spin_ros(self):
try:
# 仅在 ROS2 正常工作时才尝试 spin
if rclpy.ok():
self.executor.spin_once(timeout_sec=0.01)
latest_items = list(self.monitor.latest_data.items())
self.monitor.latest_data.clear()
for (drone_id, msg_type), data in latest_items:
for (drone_id, msg_type), data in self.monitor.latest_data.items():
self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
self.monitor.latest_data.clear()
except RuntimeError as e:
# ROS2 context 被破坏或不可用
if "Context" in str(e) or "context" in str(e).lower():
_log("WARN", f"ROS2 context 錯誤（已忽略）: {e}")
break
_log("ERROR", f"ROS spin thread error: {e}")
else:
_log("ERROR", f"ROS spin error: {e}")
traceback.print_exc()
except Exception as e:
_log("ERROR", f"ROS spin thread error: {e}")
_log("ERROR", f"ROS spin error: {e}")
traceback.print_exc()
def _stop_ros_thread(self):
self.ros_thread_running = False
ros_thread = getattr(self, 'ros_thread', None)
if ros_thread and ros_thread.is_alive():
ros_thread.join(timeout=1.0)
def closeEvent(self, event):
self._closing = True
self._restore_stream_redirector()
self._stop_ros_thread()
try:
try:
self.planner_pool.shutdown(wait=False, cancel_futures=True)
except TypeError:
self.planner_pool.shutdown(wait=False)
for group in self.mission_groups.values():
if group.executor:
group.executor.stop()

Write Preview
Loading…
Cancel
Save