Update GUI 2.3.0: JSON in Serial

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")
+            print(f"Serial receiver started on {self.port} at {self.baudrate} baud (MAVLink/JSON auto detect)")
+            if serial is None:
+                raise RuntimeError("pyserial 未安裝，無法啟動 Serial 連線")
 
-            # 創建 MAVLink 串口連接
-            self.mav = mavutil.mavlink_connection(
+            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,6 +590,7 @@ class WebSocketMavlinkReceiver(threading.Thread):
                             
                         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:
@@ -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

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,47 +1919,23 @@ 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,
+        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,
         }
-            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
+        self._submit_mission_plan(
+            group, selected_drones, drone_gps_positions,
+            target_gps, mission_type, params, context
         )
 
-            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)
     # ================================================================================
@@ -1831,7 +1965,6 @@ 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
@@ -1841,41 +1974,18 @@ class ControlStationUI(QMainWindow):
         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,
+        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,
         }
-            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
+        self._submit_mission_plan(
+            group, selected_drones, drone_gps_positions,
+            target_gps, MissionType.GRID_SWEEP, params, context
         )
-        except Exception as e:
-            self.statusBar().showMessage(f"Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
-            traceback.print_exc()
 
     # ================================================================================
     # 任務規劃 — 路徑確認 (Leader-Follower)
@@ -1917,7 +2027,6 @@ 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
@@ -1927,43 +2036,19 @@ class ControlStationUI(QMainWindow):
         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,
+        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,
         }
-            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
+        self._submit_mission_plan(
+            group, selected_drones, drone_gps_positions,
+            target_gps, MissionType.LEADER_FOLLOWER, params, context
         )
-        except Exception as e:
-            self.statusBar().showMessage(f"Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
-            traceback.print_exc()
 
     # ================================================================================
     # 任務執行回呼
@@ -2064,6 +2149,30 @@ 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)
@@ -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)")
+        except Exception as e:
+            _log("ERROR", f"Panel 更新錯誤: {e}")
+            traceback.print_exc()
 
-        finally:
-            # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪）
-            if hasattr(self, 'overview_table') and self.overview_table:
-                self.overview_table.setUpdatesEnabled(True)
-                self.overview_table.viewport().update()
+    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):
+    def _ros_spin_thread(self):
+        while self.ros_thread_running and rclpy.ok():
             try:
-            # 仅在 ROS2 正常工作时才尝试 spin
-            if rclpy.ok():
                 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()
+
+                for (drone_id, msg_type), data in latest_items:
+                    self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
             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}")
+                    break
+                _log("ERROR", f"ROS spin thread error: {e}")
                 traceback.print_exc()
             except Exception as e:
-            _log("ERROR", f"ROS spin error: {e}")
+                _log("ERROR", f"ROS spin thread 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()