Merge branch 'master' of http://140.120.108.238:49308/chiyu1468/AirTrapMine

主專案整合

src/GUI/communication.py

@@ -4,10 +4,14 @@ import math
 import re
 import threading
 from threading import Lock
+from concurrent.futures import ThreadPoolExecutor
 import asyncio
 import websockets
 import json
 import socket
+import sys
+import os
+import traceback
 from pymavlink import mavutil
 from geometry_msgs.msg import Point, Vector3
 from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -15,6 +19,23 @@ from std_msgs.msg import Float64, String
 from mavros_msgs.msg import State, VfrHud
 from mavros_msgs.srv import CommandBool, CommandTOL
 
+# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
+_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _src_path not in sys.path:
+    sys.path.insert(0, _src_path)
+
+# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
+try:
+    from fc_network_apps.longCommand import CommandLongClient
+except ImportError as e:
+    import traceback
+    print(f"⚠️  警告: 無法導入 CommandLongClient")
+    print(f"   错误: {e}")
+    print(f"   这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
+    print(f"   完整堆栈跟踪:")
+    traceback.print_exc()
+    CommandLongClient = None
+
 class DroneSignals(QObject):
     update_signal = pyqtSignal(str, str, object)  # (msg_type, drone_id, data)
 
@@ -536,44 +557,163 @@ class DroneMonitor(Node):
         
         setattr(self, f'drone_{sys_id}_subs', subs)
 
-    async def arm_drone(self, drone_id, arm):
+    # ================================================================================
-        if drone_id not in self.arm_clients:
+    # 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
-            return False
+    # ================================================================================
-        
+    MODE_MAPPING = {
-        client = self.arm_clients[drone_id]
+        "STABILIZE": 0,
-        if not client.wait_for_service(timeout_sec=1.0):
+        "ACRO": 1,
-            return False
+        "ALT_HOLD": 2,
-            
+        "AUTO": 3,
-        request = CommandBool.Request()
+        "GUIDED": 4,
-        request.value = arm
+        "LOITER": 5,
-        
+        "RTL": 6,
-        future = client.call_async(request)
+        "CIRCLE": 7,
+        "POSITION": 8,
+        "LAND": 9,
+        "OF_LOITER": 10,
+        "DRIFT": 11,
+        "SPORT": 13,
+        "FLIP": 14,
+        "AUTOTUNE": 15,
+        "POSHOLD": 16,
+        "BRAKE": 17,
+        "THROW": 18,
+        "AVOID_ADSB": 19,
+        "GUIDED_NOGPS": 20,
+        "SMART_RTL": 21,
+    }
+    # ================================================================================
+
+    async def set_mode(self, drone_id, mode_name):
+        """使用 CommandLongClient 切換無人機飛行模式"""
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            # 獲取模式對應的 custom_mode 值
+            custom_mode = self.MODE_MAPPING.get(mode_name)
+            if custom_mode is None:
+                print(f"❌ [SET_MODE] 未知模式: {mode_name}")
+                return False
+            
+            print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
+            
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
+            
+            def _call_set_mode():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.change_mode(
+                    target_sysid=sysid,
+                    custom_mode=float(custom_mode),
+                    target_compid=0,
+                    base_mode=1.0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
+            
+            result = await loop.run_in_executor(executor, _call_set_mode)
+            if result:
+                print(f"✅ [SET_MODE] 模式切換成功")
+                return True
+            else:
+                print(f"❌ [SET_MODE] 模式切換失敗")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Arm service call failed: {e}')
+            print(f"❌ [SET_MODE] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
-    async def takeoff_drone(self, drone_id, altitude=10.0):
+    async def arm_drone(self, drone_id, arm):
-        if drone_id not in self.takeoff_clients:
+        """使用 CommandLongClient 執行 ARM/DISARM"""
-            return False
+        try:
+            # 解析 drone_id 提取 sysid
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
             
-        client = self.takeoff_clients[drone_id]
+            action_name = "解鎖" if arm else "上鎖"
-        if not client.wait_for_service(timeout_sec=1.0):
+            print(f"\n📢 [ARM] {drone_id} → {action_name}")
-            return False
             
-        request = CommandTOL.Request()
+            # 在線程池中調用 CommandLongClient
-        request.altitude = altitude
+            loop = asyncio.get_event_loop()
-        request.min_pitch = 0.0
+            executor = ThreadPoolExecutor(max_workers=1)
-        request.yaw = 0.0
+            
-        
+            def _call_arm_disarm():
-        future = client.call_async(request)
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.arm_disarm(
+                    target_sysid=sysid,
+                    arm=arm,
+                    target_compid=0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
+            
+            result = await loop.run_in_executor(executor, _call_arm_disarm)
+            if result:
+                print(f"✅ [ARM] {action_name}成功")
+                return True
+            else:
+                print(f"❌ [ARM] {action_name}失敗")
+                return False
+        except Exception as e:
+            print(f"❌ [ARM] 例外錯誤: {e}")
+            traceback.print_exc()
+            return False
+
+    async def takeoff_drone(self, drone_id, altitude):
+        """使用 CommandLongClient 執行無人機起飛"""
         try:
-            response = await future
+            # 解析 drone_id 提取 sysid
-            return response.success
+            parts = drone_id.split('_')
+            if len(parts) < 2:
+                print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
+                return False
+            sysid = int(parts[-1])
+            
+            print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
+            
+            # 在線程池中調用 CommandLongClient
+            loop = asyncio.get_event_loop()
+            executor = ThreadPoolExecutor(max_workers=1)
+            
+            def _call_takeoff():
+                client = CommandLongClient() if CommandLongClient else None
+                if not client:
+                    return False
+                result = client.takeoff(
+                    target_sysid=sysid,
+                    altitude_m=float(altitude),
+                    target_compid=0,
+                    timeout_sec=2.0,
+                )
+                client.destroy_node()
+                return result.success if result else False
+            
+            result = await loop.run_in_executor(executor, _call_takeoff)
+            if result:
+                print(f"✅ [TAKEOFF] 起飛成功")
+                return True
+            else:
+                print(f"❌ [TAKEOFF] 起飛失敗")
+                return False
         except Exception as e:
-            self.get_logger().error(f'Takeoff service call failed: {e}')
+            print(f"❌ [TAKEOFF] 例外錯誤: {e}")
+            traceback.print_exc()
             return False
 
     def send_setpoint(self, drone_id, x, y, z):

src/GUI/gui.py

@@ -12,6 +12,7 @@ import asyncio
 import json
 import subprocess
 import time
+import traceback
 
 # 導入分離的類別
 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
@@ -32,15 +33,18 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '1.0.1'
+    VERSION = '2.0.3'
     
     def __init__(self):
         super().__init__()
         self.setWindowTitle(f'GCS v{self.VERSION}')
         self.resize(1400, 900)
 
-        # 初始化ROS2
+        # 初始化消息隊列（用於線程安全的 GUI 更新）
-        rclpy.init()
+        import queue
+        self.message_queue = queue.Queue()
+
+        # 初始化ROS2 Monitor（ROS2 本身在 main() 中已初始化）
         self.monitor = DroneMonitor()
         self.monitor.signals.update_signal.connect(self.update_ui)
         
@@ -53,11 +57,22 @@ class ControlStationUI(QMainWindow):
         self.timer.timeout.connect(self.spin_ros)
         self.timer.start(10)
         
+        # 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
+        # 這允許異步任務（如 arm_drone）能夠執行
+        self.asyncio_timer = QTimer()
+        self.asyncio_timer.timeout.connect(self._spin_asyncio)
+        self.asyncio_timer.start(10)  # 每 10ms 驅動一次 asyncio
+        
         # 初始化 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
         
+        # 消息隊列處理定時器（來自線程的 GUI 更新）
+        self.msg_queue_timer = QTimer()
+        self.msg_queue_timer.timeout.connect(self._process_message_queue)
+        self.msg_queue_timer.start(50)  # 每 50ms 檢查一次
+        
         # 快取消息數據，以便在沒有新消息時使用上一次的值
         self._message_cache = {}
         
@@ -164,20 +179,29 @@ class ControlStationUI(QMainWindow):
         
         # ========== 任務群組 Tab ==========
         group_header = QHBoxLayout()
+        
+        # 標題 + 收起/展開按鈕
+        title_layout = QHBoxLayout()
         group_title = QLabel("任務群組")
         group_title.setStyleSheet(
             "color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
-        group_header.addWidget(group_title)
+        title_layout.addWidget(group_title)
-        group_header.addStretch()
+        
-
+        # 收起/展開按鈕
-        add_group_btn = QPushButton("+ 新增群組")
+        self.toggle_group_btn = QPushButton("▼")
-        add_group_btn.setStyleSheet("""
+        self.toggle_group_btn.setStyleSheet("""
-            QPushButton { background-color: #4A9EFF; color: white; border: none;
+            QPushButton { background-color: #555; color: white; border: none;
-                padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
+                padding: 3px 8px; border-radius: 3px; font-size: 12px; font-weight: bold;
-            QPushButton:hover { background-color: #3A8EEF; }
+                min-width: 30px; max-width: 30px; }
+            QPushButton:hover { background-color: #666; }
         """)
-        add_group_btn.clicked.connect(self._add_mission_group)
+        self.toggle_group_btn.setToolTip("收起/展開任務群組")
-        group_header.addWidget(add_group_btn)
+        self.toggle_group_btn.clicked.connect(self._toggle_group_panel)
+        title_layout.addWidget(self.toggle_group_btn)
+        title_layout.addStretch()
+        
+        group_header.addLayout(title_layout)
+        group_header.addStretch()
 
         clear_traj_btn = QPushButton("清除軌跡")
         clear_traj_btn.setStyleSheet("""
@@ -204,6 +228,9 @@ class ControlStationUI(QMainWindow):
         self.group_tab_widget.setFixedHeight(150)
         self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
         right_layout.addWidget(self.group_tab_widget)
+        
+        # 🌟 新增：保存群組面板的展開狀態
+        self.group_panel_expanded = True
 
         # 預設建立 Group A
         self._add_mission_group()
@@ -353,6 +380,8 @@ class ControlStationUI(QMainWindow):
                 status_label.setToolTip("運行中")
                 self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
             except Exception as e:
+                print(f"❌ Serial 連接啟動失敗: {str(e)}")
+                traceback.print_exc()
                 self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
     
     def remove_serial_connection(self, conn, panel):
@@ -377,22 +406,39 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_mode_change(self, drone_id):
+        print(f"\n📢 [GUI] handle_mode_change 被调用")
+        print(f"   drone_id: {drone_id}")
+        
         # 從 active group 的 mode_combo 讀取模式
         group = self._get_active_group()
         if group:
             panel = self.group_panels.get(group.group_id)
             mode = panel.mode_combo.currentText() if panel else "GUIDED"
+            print(f"   从群组读取模式: {mode}")
         else:
             mode = "GUIDED"
+            print(f"   没有活跃群组，使用默认模式: {mode}")
+        
         loop = asyncio.get_event_loop()
         future = self.monitor.set_mode(drone_id, mode)
         loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
 
     def handle_arm(self, drone_id):
+        print(f"\n📢 [GUI] handle_arm 被調用")
+        print(f"   drone_id: {drone_id}")
         loop = asyncio.get_event_loop()
+        print(f"   獲得事件循環: {loop}")
         arm_state = not self.monitor.get_arm_state(drone_id)
-        future = self.monitor.arm_drone(drone_id, arm_state)
+        print(f"   當前鎖定狀態: {not arm_state}, 目標狀態: {arm_state}")
-        loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
+        print(f"   準備調用 arm_drone(drone_id={drone_id}, arm={arm_state})")
+        coro = self.monitor.arm_drone(drone_id, arm_state)
+        print(f"   arm_drone() 返回類型: {type(coro)}")
+        print(f"   coroutine 對象: {coro}")
+        action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
+        print(f"   準備提交到事件循環: {action_text}")
+        task = loop.create_task(self.handle_service_response(coro, action_text))
+        print(f"   task 已創建: {task}")
+        print(f"   已提交到事件循環\n")
 
     def handle_takeoff(self, drone_id):
         loop = asyncio.get_event_loop()
@@ -425,20 +471,52 @@ class ControlStationUI(QMainWindow):
             self.statusBar().showMessage("座標格式錯誤", 3000)
 
     async def handle_service_response(self, future, action):
+        print(f"\n📥 [GUI] handle_service_response 開始處理: {action}")
+        print(f"   Future/Coroutine 類型: {type(future)}")
+        print(f"   Future/Coroutine 對象: {future}")
         try:
+            print(f"   ├─ 等待 future/coroutine 完成...")
+            print(f"   └─ (這是一個阻塞等待，直到服務返回)\n")
             result = await future
+            print(f"\n   ✓ Future/Coroutine 完成，接收到返回值！")
+            print(f"   ├─ 返回值類型: {type(result)}")
+            print(f"   ├─ 返回值內容: {result}")
+            print(f"   ├─ 返回值 == True: {result == True}")
+            print(f"   ├─ 返回值 is True: {result is True}")
+            print(f"   └─ bool(返回值): {bool(result)}")
+            
+            # 詳細檢查 result 結構（用於調試 fc_network 回傳值）
+            if hasattr(result, '__dict__'):
+                print(f"   └─ 返回值屬性: {result.__dict__}")
+            
             if result:
+                print(f"\n✅ {action} 成功 (result={result})")
                 self.statusBar().showMessage(f"{action} 成功", 3000)
             else:
+                print(f"\n❌ {action} 失敗 (result={result})")
                 self.statusBar().showMessage(f"{action} 失敗", 3000)
+        except asyncio.CancelledError:
+            print(f"⚠️  {action} 被取消")
         except Exception as e:
+            print(f"❌ {action} 錯誤: {str(e)}")
+            traceback.print_exc()
             self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
 
     def handle_arm_selected(self):
+        print(f"\n📢 [GUI] handle_arm_selected 被調用")
+        selected = list(self.monitor.selected_drones)
+        print(f"   已選擇的無人機: {selected}")
         loop = asyncio.get_event_loop()
-        for drone_id in self.monitor.selected_drones:
+        for drone_id in selected:
-            future = self.monitor.arm_drone(drone_id, True)
+            print(f"   準備批次解鎖: {drone_id}")
-            loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}"))
+            coro = self.monitor.arm_drone(drone_id, True)
+            print(f"   arm_drone 返回: {coro}")
+            # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
+            asyncio.run_coroutine_threadsafe(
+                self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
+                loop
+            )
+        print(f"   handle_arm_selected 完成\n")
 
     def handle_takeoff_selected(self):
         loop = asyncio.get_event_loop()
@@ -456,6 +534,23 @@ class ControlStationUI(QMainWindow):
         self._group_counter += 1
         return gid
 
+    def _toggle_group_panel(self):
+        """🌟 收起/展開任務群組面板"""
+        if self.group_panel_expanded:
+            # 收起
+            self.group_tab_widget.setFixedHeight(0)
+            self.group_tab_widget.hide()
+            self.toggle_group_btn.setText("▶")
+            self.toggle_group_btn.setToolTip("展開任務群組")
+            self.group_panel_expanded = False
+        else:
+            # 展開
+            self.group_tab_widget.setFixedHeight(150)
+            self.group_tab_widget.show()
+            self.toggle_group_btn.setText("▼")
+            self.toggle_group_btn.setToolTip("收起任務群組")
+            self.group_panel_expanded = True
+
     def _add_mission_group(self):
         """新增一個任務群組"""
         gid = self._next_group_id()
@@ -475,6 +570,8 @@ class ControlStationUI(QMainWindow):
         panel.box_select_requested.connect(self._handle_box_select)
         panel.select_all_requested.connect(self._handle_select_all_for_group)
         panel.clear_group_requested.connect(self._handle_clear_group)
+        panel.add_group_requested.connect(self._add_mission_group)
+        panel.delete_group_requested.connect(self._handle_delete_group)
         self.group_panels[gid] = panel
 
         # 用帶顏色的 tab 標題
@@ -538,6 +635,19 @@ class ControlStationUI(QMainWindow):
             if panel:
                 panel.update_drone_list()
                 panel.update_status()
+            
+            # 同步更新左側面板的 checkbox 狀態
+            self.monitor.selected_drones = group.drone_ids.copy()
+            for drone_id in all_ids:
+                if drone_id in self.drones:
+                    checkbox = self.drones[drone_id].get_checkbox()
+                    if checkbox:
+                        checkbox.blockSignals(True)
+                        checkbox.setChecked(drone_id in group.drone_ids)
+                        checkbox.blockSignals(False)
+                # 更新 socket 群組的 checkbox 狀態
+                self.update_group_checkbox_state(self.get_socket_id(drone_id))
+            
             self.statusBar().showMessage(
                 f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
 
@@ -611,23 +721,86 @@ class ControlStationUI(QMainWindow):
 
     def _handle_group_mode_change(self, group_id, mode):
         """切換群組內所有無人機的飛行模式"""
+        print(f"\n📢 [GUI] _handle_group_mode_change 被调用", flush=True)
+        print(f"   group_id: {group_id}, mode: {mode}", flush=True)
+        
         group = self.mission_groups.get(group_id)
         if not group:
+            print(f"❌ 找不到群組: {group_id}", flush=True)
             return
+        
+        if not group.drone_ids:
+            print(f"⚠️  群組中沒有無人機", flush=True)
+            self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
+            return
+        
+        print(f"   準備為 {len(group.drone_ids)} 台無人機切換模式...", flush=True)
+        self.statusBar().showMessage(f"正在切換模式...", 1000)
+        
+        # 使用 asyncio 執行（通過事件循環）
+        async def do_mode_changes_async():
+            print(f"\n   【異步任務】開始執行模式切換", flush=True)
+            
+            for drone_id in group.drone_ids:
+                print(f"\n   【切換】{drone_id} → {mode}", flush=True)
+                try:
+                    result = await self.monitor.set_mode(drone_id, mode)
+                    
+                    if result:
+                        msg = f"✅ {drone_id} 切換成功"
+                        print(f"   {msg}", flush=True)
+                        self.message_queue.put((msg, 2000))
+                    else:
+                        msg = f"❌ {drone_id} 切換失敗"
+                        print(f"   {msg}", flush=True)
+                        self.message_queue.put((msg, 2000))
+                
+                except Exception as e:
+                    msg = f"❌ {drone_id} 錯誤: {str(e)}"
+                    print(f"   {msg}", flush=True)
+                    traceback.print_exc()
+                    self.message_queue.put((msg, 2000))
+            
+            print(f"\n   【完成】模式切換任務結束\n", flush=True)
+        
+        # 通過事件循環提交異步任務
+        print(f"   【排隊】將任務提交至事件循環", flush=True)
         loop = asyncio.get_event_loop()
-        for drone_id in group.drone_ids:
+        asyncio.run_coroutine_threadsafe(
-            future = self.monitor.set_mode(drone_id, mode)
+            do_mode_changes_async(),
-            loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}"))
+            loop
+        )
 
     def _handle_group_arm(self, group_id):
         """解鎖群組內所有無人機"""
+        print(f"\n📢 [GUI] _handle_group_arm 被調用")
+        print(f"   群組 ID: {group_id}")
         group = self.mission_groups.get(group_id)
+        print(f"   群組存在: {group is not None}")
         if not group:
+            print(f"   ⚠️ 群組不存在，返回\n")
             return
+        print(f"   群組內無人機: {group.drone_ids}")
         loop = asyncio.get_event_loop()
+        print(f"   事件循環: {loop}")
+        
         for drone_id in group.drone_ids:
-            future = self.monitor.arm_drone(drone_id, True)
+            print(f"\n   ┌─ 處理無人機: {drone_id}")
-            loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}"))
+            print(f"   ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
+            coro = self.monitor.arm_drone(drone_id, True)
+            print(f"   ├─ arm_drone 返回類型: {type(coro)}")
+            action_text = f"解鎖 {drone_id}"
+            print(f"   ├─ 準備提交到事件循環: {action_text}")
+            # 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
+            # 這是 Qt + asyncio 整合的正確方式
+            future = asyncio.run_coroutine_threadsafe(
+                self.handle_service_response(coro, action_text),
+                loop
+            )
+            print(f"   ├─ Future 已創建: {future}")
+            print(f"   └─ Future 將在事件循環中執行")
+        
+        print(f"\n   _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
 
     def _handle_group_takeoff(self, group_id, altitude):
         """群組內所有無人機起飛"""
@@ -663,23 +836,59 @@ class ControlStationUI(QMainWindow):
         if panel:
             panel.update_drone_list()
             panel.update_status()
+        
+        # 同步更新左側面板的 checkbox 狀態
+        self.monitor.selected_drones = group.drone_ids.copy()
+        for drone_id in self.drones.keys():
+            checkbox = self.drones[drone_id].get_checkbox()
+            if checkbox:
+                checkbox.blockSignals(True)
+                checkbox.setChecked(drone_id in group.drone_ids)
+                checkbox.blockSignals(False)
+            self.update_group_checkbox_state(self.get_socket_id(drone_id))
+        
         self.statusBar().showMessage(
             f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
 
     def _handle_select_all_for_group(self, group_id):
-        """全選所有可用無人機，直接分配到該群組"""
+        """全選/取消全選 - Toggle 邏輯"""
         group = self.mission_groups.get(group_id)
         if not group:
             return
+        
         other = self._get_other_assigned(group_id)
         available = {did for did in self.drones.keys() if did not in other}
-        group.drone_ids = available
+        
+        # Toggle 邏輯：如果已全選，則清空；否則全選
+        if group.drone_ids == available:
+            # 已全選 → 清空
+            group.drone_ids = set()
+            self.monitor.selected_drones.clear()
+            msg_status = "已取消全選"
+        else:
+            # 未全選 → 全選
+            group.drone_ids = available
+            self.monitor.selected_drones = group.drone_ids.copy()
+            msg_status = f"全選分配 {len(available)} 台無人機"
+        
         panel = self.group_panels.get(group_id)
         if panel:
             panel.update_drone_list()
             panel.update_status()
+            # 更新按鈕文本
+            panel.set_all_select_state(group.drone_ids == available)
+        
+        # 同步更新左側面板的 checkbox 狀態
+        for drone_id in self.drones.keys():
+            checkbox = self.drones[drone_id].get_checkbox()
+            if checkbox:
+                checkbox.blockSignals(True)
+                checkbox.setChecked(drone_id in group.drone_ids)
+                checkbox.blockSignals(False)
+            self.update_group_checkbox_state(self.get_socket_id(drone_id))
+        
         self.statusBar().showMessage(
-            f"Group {group_id}: 全選分配 {len(available)} 台無人機", 3000)
+            f"Group {group_id}: {msg_status}", 3000)
 
     def _handle_clear_group(self, group_id):
         """清除群組的無人機分配"""
@@ -696,9 +905,59 @@ class ControlStationUI(QMainWindow):
             panel.update_drone_list()
             panel.update_status()
             panel.clear_mission_info()
+        
+        # 同步更新左側面板的 checkbox 狀態（全部取消勾選）
+        self.monitor.selected_drones.clear()
+        for drone_id in self.drones.keys():
+            checkbox = self.drones[drone_id].get_checkbox()
+            if checkbox:
+                checkbox.blockSignals(True)
+                checkbox.setChecked(False)
+                checkbox.blockSignals(False)
+            self.update_group_checkbox_state(self.get_socket_id(drone_id))
+        
         self.statusBar().showMessage(
             f"Group {group_id}: 已清除分組", 3000)
 
+    def _handle_delete_group(self, group_id):
+        """刪除一個任務群組"""
+        if group_id not in self.mission_groups:
+            self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
+            return
+        
+        # 停止群組的執行（如果有）
+        group = self.mission_groups[group_id]
+        if group.executor:
+            group.executor.stop()
+        
+        # 移除地圖上的任務計畫
+        self.drone_map.clear_mission_plan_for_group(group_id)
+        
+        # 移除 tab
+        for i in range(self.group_tab_widget.count()):
+            if f"Group {group_id}" in self.group_tab_widget.tabText(i):
+                self.group_tab_widget.removeTab(i)
+                break
+        
+        # 移除資料
+        del self.mission_groups[group_id]
+        if group_id in self.group_panels:
+            del self.group_panels[group_id]
+        
+        # 更新 active group
+        if self.active_group_id == group_id:
+            if self.group_tab_widget.count() > 0:
+                self.group_tab_widget.setCurrentIndex(0)
+                # 更新 active_group_id 為當前 tab 的群組
+                for gid, panel in self.group_panels.items():
+                    if panel == self.group_tab_widget.currentWidget().widget():
+                        self.active_group_id = gid
+                        break
+            else:
+                self.active_group_id = None
+        
+        self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
+
     def _on_group_mission_completed(self, group_id):
         """群組任務完成回呼"""
         panel = self.group_panels.get(group_id)
@@ -739,23 +998,111 @@ class ControlStationUI(QMainWindow):
     # ================================================================================
 
     def handle_group_selection(self, socket_id, state):
+        """處理 socket 群組 checkbox 的勾選/取消勾選
+        
+        這個方法在用戶點擊 socket 群組的 checkbox 時被調用。
+        需要同時更新：
+        1. 該 socket 下所有無人機的 checkbox
+        2. self.monitor.selected_drones（用於控制面板同步）
+        3. 右侧活躍群組的無人機分配（新增）
+        
+        參數：
+            socket_id: socket ID (str)
+            state: Qt.CheckState 的整數值 (0=Unchecked, 1=PartiallyChecked, 2=Checked)
+        """
+        print(f"\n📢 [GUI] handle_group_selection 被調用", flush=True)
+        print(f"   socket_id: {socket_id}, state: {state}", flush=True)
+        print(f"   state 類型: {type(state)}", flush=True)
+        
+        # 獲取該 socket 下所有無人機
         group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
-        is_checked = state == Qt.CheckState.Checked.value
+        print(f"   該 socket 下的無人機: {group_drones}", flush=True)
+        
+        # 判斷是否勾選（只有 state == 2 時才是 Checked）
+        is_checked = (state == 2)  # Qt.CheckState.Checked.value == 2
+        print(f"   is_checked: {is_checked}", flush=True)
+        
+        # 更新該 socket 下所有無人機的 checkbox 狀態
         for drone_id in group_drones:
             checkbox = self.drones[drone_id].get_checkbox()
             if checkbox:
+                print(f"   └─ 更新 {drone_id}: setChecked({is_checked})", flush=True)
                 checkbox.blockSignals(True)
                 checkbox.setChecked(is_checked)
                 checkbox.blockSignals(False)
-                if is_checked: self.monitor.selected_drones.add(drone_id)
+                
-                else: self.monitor.selected_drones.discard(drone_id)
+                # 同時更新 monitor.selected_drones 以同步控制面板
+                if is_checked:
+                    self.monitor.selected_drones.add(drone_id)
+                else:
+                    self.monitor.selected_drones.discard(drone_id)
+        
+        # 👇 新增：同步更新右侧活躍群組的無人機分配
+        if self.active_group_id:
+            group = self.mission_groups.get(self.active_group_id)
+            panel = self.group_panels.get(self.active_group_id)
+            if group and panel:
+                print(f"   ├─ 同步右侧群組 {self.active_group_id}", flush=True)
+                if is_checked:
+                    # 勾選時：將該 socket 下的無人機添加到活躍群組
+                    for drone_id in group_drones:
+                        group.drone_ids.add(drone_id)
+                    print(f"   │  └─ 添加到群組: {group_drones}", flush=True)
+                else:
+                    # 取消勾選時：從活躍群組移除該 socket 下的無人機
+                    for drone_id in group_drones:
+                        group.drone_ids.discard(drone_id)
+                    print(f"   │  └─ 從群組移除: {group_drones}", flush=True)
+                
+                # 更新右側群組面板的顯示
+                panel.update_drone_list()
+                panel.update_status()
+                print(f"   │  └─ 已更新右侧群組面板", flush=True)
+        
+        print(f"   最終 selected_drones: {self.monitor.selected_drones}", flush=True)
+        print(f"✓ handle_group_selection 完成\n", flush=True)
 
     def handle_drone_selection(self, drone_id, state):
-        if state == Qt.CheckState.Checked.value:
+        is_checked = state == Qt.CheckState.Checked.value
+        if is_checked:
             self.monitor.selected_drones.add(drone_id)
         else:
             self.monitor.selected_drones.discard(drone_id)
         self.update_group_checkbox_state(self.get_socket_id(drone_id))
+        
+        # 同步更新任務群組的無人機分配狀態
+        # 遍歷所有任務群組，更新已分配的無人機列表顯示
+        if not is_checked:
+            # 取消勾選時：從所有包含該無人機的群組中移除
+            for group_id, group in self.mission_groups.items():
+                if drone_id in group.drone_ids:
+                    group.drone_ids.discard(drone_id)
+                    panel = self.group_panels.get(group_id)
+                    if panel:
+                        panel.update_drone_list()
+                        panel.update_status()
+                        # 更新全選按鈕狀態
+                        other = self._get_other_assigned(group_id)
+                        available = {did for did in self.drones.keys() if did not in other}
+                        panel.set_all_select_state(group.drone_ids == available)
+        else:
+            # 勾選時：檢查該無人機是否已分配給其他群組，若未分配則添加到當前活躍群組
+            is_already_assigned = any(
+                drone_id in group.drone_ids 
+                for group in self.mission_groups.values()
+            )
+            if not is_already_assigned and self.active_group_id:
+                # 無人機未被分配給任何群組，可以添加到當前活躍群組
+                group = self.mission_groups.get(self.active_group_id)
+                panel = self.group_panels.get(self.active_group_id)
+                if group and panel:
+                    group.drone_ids.add(drone_id)
+                    panel.update_drone_list()
+                    panel.update_status()
+                    # 更新全選按鈕狀態
+                    other = self._get_other_assigned(self.active_group_id)
+                    available = {did for did in self.drones.keys() if did not in other}
+                    panel.set_all_select_state(group.drone_ids == available)
 
     def update_group_checkbox_state(self, socket_id):
         group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
@@ -890,7 +1237,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -962,7 +1308,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -1042,7 +1387,6 @@ class ControlStationUI(QMainWindow):
             )
         except Exception as e:
             self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
-            import traceback
             traceback.print_exc()
 
     # ================================================================================
@@ -1180,7 +1524,6 @@ class ControlStationUI(QMainWindow):
         self._map_update_count += 1
         now = time.time()
         if now - self._map_update_time >= 1.0:
-            print(f"[Panel/Map Update] {self._map_update_count} Hz")
             self._map_update_time = now
             self._map_update_count = 0
         
@@ -1314,32 +1657,126 @@ class ControlStationUI(QMainWindow):
 
 
 
+    def _process_message_queue(self):
+        """處理來自後台線程的消息隊列（更新 GUI 狀態欄）"""
+        try:
+            while True:
+                try:
+                    message, duration = self.message_queue.get_nowait()
+                    self.statusBar().showMessage(message, duration)
+                except:
+                    break
+        except Exception as e:
+            print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
+            traceback.print_exc()
+
+    def _spin_asyncio(self):
+        """驅動 asyncio 事件循環，允許異步任務執行
+        
+        關鍵修復：asyncio 事件循環不會自動運行。
+        這個定時器會定期執行事件循環，讓 run_coroutine_threadsafe() 提交的任務能夠執行。
+        """
+        try:
+            loop = asyncio.get_event_loop()
+            if loop and not loop.is_closed() and not loop.is_running():
+                # 執行事件循環直到沒有掛起的任務或超時
+                # 使用 run_until_complete() 配合一個快速返回的 coroutine
+                loop.run_until_complete(asyncio.sleep(0))
+        except Exception as e:
+            # 靜默忽略任何錯誤，防止 Qt 定時器出現異常
+            # 但仍然打印詳細的堆棧跟踪以便調試
+            traceback.print_exc()
+
     def spin_ros(self):
         try:
-            self.executor.spin_once(timeout_sec=0.01)
+            # 仅在 ROS2 正常工作时才尝试 spin
-            for (drone_id, msg_type), data in self.monitor.latest_data.items():
+            if rclpy.ok():
-                self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
+                self.executor.spin_once(timeout_sec=0.01)
-            self.monitor.latest_data.clear()
+                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():
+                print(f"⚠️ ROS2 context 錯誤（忽略）: {e}", flush=True)
+            else:
+                print(f"ROS spin error: {e}", flush=True)
+                traceback.print_exc()
         except Exception as e:
-            print(f"ROS spin error: {e}")
+            print(f"ROS spin error: {e}", flush=True)
+            traceback.print_exc()
 
     def closeEvent(self, event):
-        for group in self.mission_groups.values():
+        try:
-            if group.executor:
+            for group in self.mission_groups.values():
-                group.executor.stop()
+                if group.executor:
-        self.command_sender.close()
+                    group.executor.stop()
-        for receiver in self.udp_receivers:
+            self.command_sender.close()
-            receiver.stop()
+            for receiver in self.udp_receivers:
-        for receiver in self.monitor.ws_receivers:
+                receiver.stop()
-            receiver.stop()
+            for receiver in self.monitor.ws_receivers:
-        self.monitor.destroy_node()
+                receiver.stop()
-        self.executor.shutdown()
+            self.monitor.destroy_node()
-        rclpy.shutdown()
+            self.executor.shutdown()
+        except Exception as e:
+            print(f"⚠️ 清理資源時出錯: {e}", flush=True)
+            traceback.print_exc()
+        
+        # 安全地 shutdown ROS2
+        try:
+            if rclpy.ok():
+                rclpy.shutdown()
+        except RuntimeError as e:
+            print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
+            traceback.print_exc()
+        
         event.accept()
 
 
+def main():
+    """
+    GUI 應用程式的主入口點
+    
+    標準 ROS2 + Qt 架構：
+    1. 在最外層/最前面只做一次 rclpy.init()
+    2. 啟動 Qt 應用程式
+    3. 在 finally 中做 rclpy.shutdown()
+    
+    這樣可以確保所有 ROS2 相關操作都共享同一個初始化狀態
+    """
+    # 第一步：在最外層只初始化一次 ROS2（終極防護）
+    # 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
+    print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True)
+    if not rclpy.ok():
+        print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True)
+        rclpy.init()
+        print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True)
+    else:
+        print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True)
+    
+    try:
+        # 第二步：啟動 Qt 應用程式
+        print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True)
+        app = QApplication(sys.argv)
+        station = ControlStationUI()
+        station.show()
+        print("✓ [GUI 主程式] 應用程式已啟動", flush=True)
+        
+        # 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
+        print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True)
+        sys.exit(app.exec())
+        
+    finally:
+        # 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
+        # 這裡確保 ROS2 被正確且安全地關閉
+        print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True)
+        if rclpy.ok():
+            rclpy.shutdown()
+            print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True)
+        else:
+            print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True)
+        print("✓ [GUI 主程式] 應用程式完全退出", flush=True)
+
+
 if __name__ == '__main__':
-    app = QApplication(sys.argv)
+    main()
-    station = ControlStationUI()
-    station.show()
-    app.exec()

src/GUI/mission_group.py

@@ -146,6 +146,8 @@ class GroupPanel(QWidget):
     box_select_requested = pyqtSignal(str)             # group_id — 框選直接分配
     select_all_requested = pyqtSignal(str)             # group_id — 全選直接分配
     clear_group_requested = pyqtSignal(str)            # group_id — 清除分組
+    add_group_requested = pyqtSignal()                 # 新增群組
+    delete_group_requested = pyqtSignal(str)           # group_id — 刪除群組
 
     BUTTON_STYLE = """
         QPushButton {{ background-color: {bg}; color: {fg}; border: none;
@@ -157,6 +159,8 @@ class GroupPanel(QWidget):
     def __init__(self, group: MissionGroup, parent=None):
         super().__init__(parent)
         self.group = group
+        self._is_all_selected = False  # 追蹤全選狀態
+        self.all_btn_ref = None  # 保存全選按鈕的參考
         self._build_ui()
 
     def _make_sep(self):
@@ -296,7 +300,7 @@ class GroupPanel(QWidget):
         mid.addLayout(mid_body)
 
         # ============================
-        # 選取與分組（2x2 按鈕）
+        # 選取與分組（3x2 按鈕）
         # ============================
         right = QVBoxLayout()
         right.setSpacing(3)
@@ -310,9 +314,28 @@ class GroupPanel(QWidget):
         self.drone_list_label.setWordWrap(True)
         right.addWidget(self.drone_list_label)
 
-        # 2x2 按鈕網格
+        # 3x2 按鈕網格：第一行 框選 全選 新增群組
         grid_r1 = QHBoxLayout()
         grid_r1.setSpacing(3)
+        box_btn = QPushButton("框選")
+        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        box_btn.clicked.connect(
+            lambda: self.box_select_requested.emit(self.group.group_id))
+        all_btn = QPushButton("全選/取消")
+        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
+        all_btn.clicked.connect(self._on_all_select_clicked)
+        self.all_btn_ref = all_btn  # 保存按鈕參考（備用）
+        add_group_btn = QPushButton("+ 新增群組")
+        add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
+        add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
+        grid_r1.addWidget(box_btn)
+        grid_r1.addWidget(all_btn)
+        grid_r1.addWidget(add_group_btn)
+        right.addLayout(grid_r1)
+
+        # 第二行 編輯分配 清除分組 刪除群組
+        grid_r2 = QHBoxLayout()
+        grid_r2.setSpacing(3)
         assign_btn = QPushButton("編輯分配")
         assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
         assign_btn.clicked.connect(
@@ -321,22 +344,13 @@ class GroupPanel(QWidget):
         clear_btn.setStyleSheet(BTN.format(bg='#777', fg='white', hover='#888'))
         clear_btn.clicked.connect(
             lambda: self.clear_group_requested.emit(self.group.group_id))
-        grid_r1.addWidget(assign_btn)
+        delete_group_btn = QPushButton("刪除群組")
-        grid_r1.addWidget(clear_btn)
+        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
-        right.addLayout(grid_r1)
+        delete_group_btn.clicked.connect(
-
+            lambda: self.delete_group_requested.emit(self.group.group_id))
-        grid_r2 = QHBoxLayout()
+        grid_r2.addWidget(assign_btn)
-        grid_r2.setSpacing(3)
+        grid_r2.addWidget(clear_btn)
-        box_btn = QPushButton("框選")
+        grid_r2.addWidget(delete_group_btn)
-        box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
-        box_btn.clicked.connect(
-            lambda: self.box_select_requested.emit(self.group.group_id))
-        all_btn = QPushButton("全選")
-        all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
-        all_btn.clicked.connect(
-            lambda: self.select_all_requested.emit(self.group.group_id))
-        grid_r2.addWidget(box_btn)
-        grid_r2.addWidget(all_btn)
         right.addLayout(grid_r2)
 
         right.addStretch()
@@ -410,13 +424,15 @@ class GroupPanel(QWidget):
         self.type_combo.currentTextChanged.connect(self._update_param_visibility)
 
         # ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
-        cols.addLayout(left, 1)
+        # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
+        cols.addLayout(left, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(mid, 1)
+        cols.addLayout(mid, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(param_col, 1)
+        cols.addLayout(param_col, 0)
         cols.addWidget(self._make_sep())
-        cols.addLayout(right, 1)
+        cols.addLayout(right, 0)
+        cols.addStretch()  # 填充剩餘空間，使四列置左
 
         layout.addLayout(cols)
 
@@ -451,6 +467,14 @@ class GroupPanel(QWidget):
             self.status_label.setStyleSheet(
                 f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
 
+    def _on_all_select_clicked(self):
+        """全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
+        self.select_all_requested.emit(self.group.group_id)
+    
+    def set_all_select_state(self, is_selected):
+        """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
+        self._is_all_selected = is_selected
+
     def _update_param_visibility(self, _=None):
         """根據當前任務類型，顯示/隱藏對應的參數列"""
         mission_type = self.type_combo.currentText()