From d2a5f6fad70466225aa234388c1116c2f081b01a Mon Sep 17 00:00:00 2001 From: ken910606 Date: Wed, 6 May 2026 20:58:18 +0800 Subject: [PATCH] Update GUI 2.3.0: JSON in Serial --- src/GUI/communication.py | 352 +++++++++++++++++++++------- src/GUI/gui.py | 491 ++++++++++++++++++++++++--------------- 2 files changed, 575 insertions(+), 268 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index c2298a8..0ef573d 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -12,6 +12,10 @@ 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 @@ -61,6 +65,153 @@ 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): @@ -184,8 +335,8 @@ class UDPMavlinkReceiver(threading.Thread): """停止接收器""" self.running = False -class SerialMavlinkReceiver(threading.Thread): - """串口 MAVLink 接收器""" +class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): + """串口遙測接收器，可自動處理 MAVLink 或 WebSocket 格式 JSON。""" def __init__(self, port, baudrate, signals, connection_name, monitor=None): super().__init__(daemon=True) self.port = port @@ -194,47 +345,125 @@ class SerialMavlinkReceiver(threading.Thread): 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.mav = None + 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() def run(self): - """執行串口接收循環""" + """執行串口接收循環。""" self.running = True try: - print(f"Serial MAVLink receiver started on {self.port} at {self.baudrate} baud") - - # 創建 MAVLink 串口連接 - self.mav = mavutil.mavlink_connection( + print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)") + if serial is None: + raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線") + + self.serial_conn = serial.Serial( self.port, - baud=self.baudrate, - source_system=255 + self.baudrate, + timeout=0.2 ) - - 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: - msg = self.mav.recv_match(blocking=True, timeout=1.0) - if msg is None: + chunk = self.serial_conn.read(256) + if not chunk: continue - - self.process_mavlink_message(msg) - + + for raw_byte in chunk: + byte = bytes([raw_byte]) + self._process_json_byte(byte) + self._process_mavlink_byte(byte) + except Exception as e: if self.running: - print(f"Error receiving MAVLink message from serial: {e}") - + print(f"Error receiving serial telemetry: {e}") + except Exception as e: print(f"Serial receiver error: {e}") finally: - if self.mav: + if self.serial_conn: try: - self.mav.close() - except: + self.serial_conn.close() + except Exception: 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: @@ -317,15 +546,23 @@ class SerialMavlinkReceiver(threading.Thread): def stop(self): """停止接收器""" self.running = False + if self.serial_conn: + try: + self.serial_conn.close() + except Exception: + pass -class WebSocketMavlinkReceiver(threading.Thread): +class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): """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.running = False + 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.max_retries = 5 self.base_delay = 1.0 @@ -353,7 +590,8 @@ class WebSocketMavlinkReceiver(threading.Thread): try: data = json.loads(message) - data['_connection_source'] = self.connection_name + if isinstance(data, dict): + data['_connection_source'] = self.connection_name self.process_websocket_message(data) except json.JSONDecodeError as e: print(f"WebSocket {self.connection_name} JSON decode error: {e}") @@ -383,57 +621,7 @@ class WebSocketMavlinkReceiver(threading.Thread): def process_websocket_message(self, data): """處理 WebSocket 訊息""" - 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}") + self.process_json_telemetry_message(data) def stop(self): """停止接收器""" @@ -1101,10 +1289,10 @@ class DroneMonitor(Node): } def start_serial_connection(self, port='/dev/ttyUSB0', baudrate=57600): - """啟動串口 MAVLink 連接""" + """啟動串口遙測連接（自動辨識 MAVLink / JSON）""" 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") + _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud (MAVLink/JSON)") return receiver diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 72d5af4..cafe7d5 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -14,6 +14,8 @@ import subprocess import time import traceback import re +import threading +from concurrent.futures import ThreadPoolExecutor def _log(level, message): @@ -143,7 +145,9 @@ class ToggleSwitch(QWidget): class ControlStationUI(QMainWindow): - VERSION = '2.2.0' + planning_finished = pyqtSignal(object) + + VERSION = '2.3.0' FONT_SCALE_MIN = 70 FONT_SCALE_MAX = 180 FONT_SCALE_DEFAULT = 100 @@ -152,6 +156,7 @@ class ControlStationUI(QMainWindow): super().__init__() self.setWindowTitle(f'GCS v{self.VERSION}') self.resize(1400, 900) + self._closing = False # 初始化消息隊列（用於線程安全的 GUI 更新） import queue @@ -168,10 +173,16 @@ class ControlStationUI(QMainWindow): # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加 self.monitor.executor = self.executor - # 定时处理ROS事件 - self.timer = QTimer() - self.timer.timeout.connect(self.spin_ros) - self.timer.start(10) + # 在背景執行緒處理 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) # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！） # 這允許異步任務（如 arm_drone）能夠執行 @@ -179,7 +190,7 @@ class ControlStationUI(QMainWindow): self.asyncio_timer.timeout.connect(self._spin_asyncio) self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio - # 初始化 panel 和 map 更新（10Hz） + # 初始化 panel 更新（10Hz） self.panel_map_timer = QTimer() self.panel_map_timer.timeout.connect(self._update_panel_and_map) self.panel_map_timer.start(100) # 10Hz @@ -191,6 +202,7 @@ class ControlStationUI(QMainWindow): # 快取消息數據，以便在沒有新消息時使用上一次的值 self._message_cache = {} + self._overview_cache = {} self.message_history = [] self.max_message_history = 500 @@ -217,6 +229,17 @@ 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 = [] @@ -726,7 +749,7 @@ class ControlStationUI(QMainWindow): self.message_history = self.message_history[-self.max_message_history:] if hasattr(self, 'message_history_view'): - self.message_history_view.setPlainText("\n".join(self.message_history)) + self.message_history_view.appendPlainText(entry) scrollbar = self.message_history_view.verticalScrollBar() scrollbar.setValue(scrollbar.maximum()) @@ -800,7 +823,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']) + receiver = WebSocketMavlinkReceiver(conn['url'], self.monitor.signals, conn['name'], self.monitor) receiver.start() self.monitor.ws_receivers.append(receiver) conn['receiver'] = receiver @@ -832,7 +855,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']) + receiver = UDPMavlinkReceiver(conn['ip'], conn['port'], self.monitor.signals, conn['name'], self.monitor) receiver.start() self.udp_receivers.append(receiver) conn['receiver'] = receiver @@ -1733,6 +1756,141 @@ 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() @@ -1761,46 +1919,22 @@ 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 - - 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, - } - 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) + drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) + if drone_gps_positions is None: + return - 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() + 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, + } + self._submit_mission_plan( + group, selected_drones, drone_gps_positions, + target_gps, mission_type, params, context + ) # ================================================================================ # 任務規劃 — 矩形選取 (Grid Sweep) @@ -1831,51 +1965,27 @@ 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 - - target_lat = sum(c[0] for c in rect_corners) / 4 - target_lon = sum(c[1] for c in rect_corners) / 4 - target_gps = (target_lat, target_lon, base_alt) - - params['rect_corners'] = rect_corners - 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, - } - 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) + drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) + if drone_gps_positions is None: + return - 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() + target_lat = sum(c[0] for c in rect_corners) / 4 + target_lon = sum(c[1] for c in rect_corners) / 4 + 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, + } + self._submit_mission_plan( + group, selected_drones, drone_gps_positions, + target_gps, MissionType.GRID_SWEEP, params, context + ) # ================================================================================ # 任務規劃 — 路徑確認 (Leader-Follower) @@ -1917,53 +2027,28 @@ 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 - - target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) - target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints) - target_gps = (target_lat, target_lon, base_alt) - - params['route_waypoints'] = route_waypoints - 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, - } - 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) + drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) + if drone_gps_positions is None: + return - 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() + target_lat = sum(p[0] for p in route_waypoints) / len(route_waypoints) + target_lon = sum(p[1] for p in route_waypoints) / len(route_waypoints) + 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, + } + self._submit_mission_plan( + group, selected_drones, drone_gps_positions, + target_gps, MissionType.LEADER_FOLLOWER, params, context + ) # ================================================================================ # 任務執行回呼 @@ -2063,6 +2148,30 @@ class ControlStationUI(QMainWindow): if not hasattr(self, 'overview_table') or self.overview_table is None: return 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 @@ -2100,7 +2209,7 @@ class ControlStationUI(QMainWindow): self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) def _update_panel_and_map(self): - """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪""" + """定時更新 panel / table，批量更新 UI 以避免過度重繪。""" if not hasattr(self, '_message_cache') or not self._message_cache: return @@ -2115,10 +2224,6 @@ 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() @@ -2144,8 +2249,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.update_overview_table(drone_id, 'mode', mode) - self.update_overview_table(drone_id, 'armed', arm_text) + self.queue_overview_update(drone_id, 'mode', mode) + self.queue_overview_update(drone_id, 'armed', arm_text) elif msg_type == 'battery': voltage = data.get('voltage', 16) @@ -2158,39 +2263,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.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V") + self.queue_overview_update(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.update_overview_table(drone_id, 'altitude', text) + self.queue_overview_update(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.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}") + self.queue_overview_update(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.update_overview_table(drone_id, 'loss_rate', text) + self.queue_overview_update(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.update_overview_table(drone_id, 'ping', text) + self.queue_overview_update(drone_id, 'ping', text) elif msg_type == 'velocity': - self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") + self.queue_overview_update(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.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}°") + 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}°") panel._last_roll = roll panel._last_pitch = pitch if hasattr(panel, 'update_attitude'): @@ -2205,8 +2310,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.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") - self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") + self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°") + self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°") elif msg_type == 'hud': hud_data = data @@ -2218,30 +2323,31 @@ class ControlStationUI(QMainWindow): hud_alt = hud_data.get('alt', 0) climb = hud_data.get('climb', 0) - 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.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_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)") - - finally: - # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪） - if hasattr(self, 'overview_table') and self.overview_table: - self.overview_table.setUpdatesEnabled(True) - self.overview_table.viewport().update() + 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) @@ -2275,28 +2381,41 @@ class ControlStationUI(QMainWindow): # 但仍然打印詳細的堆棧跟踪以便調試 traceback.print_exc() - def spin_ros(self): - try: - # 仅在 ROS2 正常工作时才尝试 spin - if rclpy.ok(): + def _ros_spin_thread(self): + while self.ros_thread_running and rclpy.ok(): + try: self.executor.spin_once(timeout_sec=0.01) - for (drone_id, msg_type), data in self.monitor.latest_data.items(): - self.monitor.signals.update_signal.emit(msg_type, drone_id, data) + + latest_items = list(self.monitor.latest_data.items()) 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}") - else: - _log("ERROR", f"ROS spin error: {e}") + + for (drone_id, msg_type), data in latest_items: + self.monitor.signals.update_signal.emit(msg_type, drone_id, data) + except RuntimeError as e: + 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}") + traceback.print_exc() + except Exception as e: + _log("ERROR", f"ROS spin thread error: {e}") traceback.print_exc() - except Exception as 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()