Merge remote-tracking branch 'origin/master' into wenchun

src/GUI/communication.py

@@ -467,14 +467,13 @@ class DroneMonitor(Node):
         self.serial_receivers = []
         
         # ================================================================================
-        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client）
         # ================================================================================
-        self.command_long_client = None
-        try:
-            self.command_long_client = CommandLongClient()
-        except Exception as e:
-            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
-            self.command_long_client = None
+        # 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件
+        self.command_long_clients = {}  # {drone_id: CommandLongClient}
+        self.client_lock = Lock()  # 保護 clients 字典的訪問
+        self.client_counter = 0  # 用於生成唯一的 client 節點名稱
+        self.executor = None  # 將在 gui.py 中設置，用於添加新的 clients
         # ================================================================================
         
         # 主题检测定时器
@@ -501,6 +500,38 @@ class DroneMonitor(Node):
             
             return self.socket_id_mapping[original_socket_id]
        
+    def get_or_create_client(self, drone_id):
+        """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件"""
+        with self.client_lock:
+            if drone_id not in self.command_long_clients:
+                try:
+                    # 生成唯一的 client 節點名稱
+                    self.client_counter += 1
+                    unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}"
+                    client = CommandLongClient(node_name=unique_name)
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})")
+                    
+                    # 將新 client 添加到主執行器（這樣它的回調才能被處理）
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                    
+                except TypeError:
+                    # 舊版 CommandLongClient 不支持 node_name 參數，使用預設
+                    client = CommandLongClient()
+                    self.command_long_clients[drone_id] = client
+                    print(f"   ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)")
+                    
+                    if self.executor:
+                        self.executor.add_node(client)
+                        print(f"   ✓ 將 {drone_id} 的 client 添加到主執行器")
+                        
+                except Exception as e:
+                    print(f"⚠️  無法為 {drone_id} 創建 CommandLongClient: {e}")
+                    return None
+            return self.command_long_clients[drone_id]
+    
     def scan_topics(self):
         topics = self.get_topic_names_and_types()
         drone_pattern = re.compile(r'/fc_network/vehicle/(sys\d+)/(\w+)')
@@ -602,7 +633,7 @@ class DroneMonitor(Node):
     # ================================================================================
 
     async def set_mode(self, drone_id, mode_name):
-        """使用 CommandLongClient 切換無人機飛行模式"""
+        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -619,30 +650,26 @@ class DroneMonitor(Node):
             
             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:
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [SET_MODE] CommandLongClient 無法初始化")
+                return False
+            
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
+            )
+            
+            if result and result.success:
                 print(f"✅ [SET_MODE] 模式切換成功")
                 return True
             else:
-                print(f"❌ [SET_MODE] 模式切換失敗")
+                print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
             print(f"❌ [SET_MODE] 例外錯誤: {e}")
@@ -662,16 +689,18 @@ class DroneMonitor(Node):
             action_name = "解鎖" if arm else "上鎖"
             print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-            if not self.command_long_client:
-                print(f"❌ [ARM] CommandLongClient 未初始化")
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [ARM] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.arm_disarm_async(
+            result = await client.arm_disarm_async(
                 target_sysid=sysid,
                 arm=arm,
                 target_compid=0,
-                timeout_sec=2.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
             )
             
             if result and result.success:
@@ -697,16 +726,18 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
             
-            if not self.command_long_client:
-                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+            # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件）
+            client = self.get_or_create_client(drone_id)
+            if not client:
+                print(f"❌ [TAKEOFF] CommandLongClient 無法初始化")
                 return False
             
             # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
-            result = await self.command_long_client.takeoff_async(
+            result = await client.takeoff_async(
                 target_sysid=sysid,
                 altitude_m=float(altitude),
                 target_compid=0,
-                timeout_sec=2.0,
+                timeout_sec=5.0,  # 增加超時時間以提高多機操作的可靠性
             )
             
             if result and result.success:

src/GUI/gui.py

@@ -33,7 +33,7 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.0.4'
+    VERSION = '2.0.6'
     
     def __init__(self):
         super().__init__()
@@ -52,9 +52,8 @@ class ControlStationUI(QMainWindow):
         self.executor = rclpy.executors.SingleThreadedExecutor()
         self.executor.add_node(self.monitor)
         
-        # 添加 CommandLongClient 到執行器（這樣它的回調才能被處理）
-        if self.monitor.command_long_client:
-            self.executor.add_node(self.monitor.command_long_client)
+        # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加
+        self.monitor.executor = self.executor
         
         # 定时处理ROS事件
         self.timer = QTimer()
@@ -588,6 +587,9 @@ class ControlStationUI(QMainWindow):
         self.active_group_id = gid
         self.statusBar().showMessage(f"已新增 Group {gid}", 2000)
         
+        # 更新刪除按鈕的啟用/禁用狀態
+        self._update_delete_buttons_state()
+
     def _on_group_tab_changed(self, index):
         """Tab 切換時更新 active group 並同步地圖模式"""
         if index < 0:
@@ -925,6 +927,11 @@ class ControlStationUI(QMainWindow):
 
     def _handle_delete_group(self, group_id):
         """刪除一個任務群組"""
+        # 檢查是否只有一個群組，如果是就禁止刪除
+        if len(self.mission_groups) <= 1:
+            self.statusBar().showMessage("⚠️ 至少需要保留一個群組！", 3000)
+            return
+        
         if group_id not in self.mission_groups:
             self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
             return
@@ -962,6 +969,17 @@ class ControlStationUI(QMainWindow):
         
         self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
         
+        # 更新刪除按鈕的啟用/禁用狀態
+        self._update_delete_buttons_state()
+
+    def _update_delete_buttons_state(self):
+        """根據群組數量，更新所有群組的刪除按鈕啟用/禁用狀態"""
+        # 如果只有一個群組，禁用該群組的刪除按鈕
+        # 如果有多個群組，啟用所有刪除按鈕
+        should_enable = len(self.mission_groups) > 1
+        for gid, panel in self.group_panels.items():
+            panel.set_delete_enabled(should_enable)
+
     def _on_group_mission_completed(self, group_id):
         """群組任務完成回呼"""
         panel = self.group_panels.get(group_id)
@@ -1723,10 +1741,10 @@ class ControlStationUI(QMainWindow):
             # Clean up serial receivers
             for receiver in self.monitor.serial_receivers:
                 receiver.stop()
-            # Clean up CommandLongClient
-            if self.monitor.command_long_client:
+            # Clean up all CommandLongClient instances
+            for drone_id, client in self.monitor.command_long_clients.items():
                 try:
-                    self.monitor.command_long_client.destroy_node()
+                    client.destroy_node()
                 except:
                     pass
             self.monitor.destroy_node()

src/GUI/mission_group.py

@@ -344,13 +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))
-        delete_group_btn = QPushButton("刪除群組")
-        delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
-        delete_group_btn.clicked.connect(
+        self.delete_group_btn = QPushButton("刪除群組")
+        self.delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
+        self.delete_group_btn.clicked.connect(
             lambda: self.delete_group_requested.emit(self.group.group_id))
         grid_r2.addWidget(assign_btn)
         grid_r2.addWidget(clear_btn)
-        grid_r2.addWidget(delete_group_btn)
+        grid_r2.addWidget(self.delete_group_btn)
         right.addLayout(grid_r2)
 
         right.addStretch()
@@ -475,6 +475,10 @@ class GroupPanel(QWidget):
         """外部設置全選狀態（按鈕文本保持「全選/取消」）"""
         self._is_all_selected = is_selected
     
+    def set_delete_enabled(self, enabled):
+        """啟用或禁用刪除群組按鈕"""
+        self.delete_group_btn.setEnabled(enabled)
+
     def _update_param_visibility(self, _=None):
         """根據當前任務類型，顯示/隱藏對應的參數列"""
         mission_type = self.type_combo.currentText()

src/fc_network_apps/longCommand.py

@@ -4,6 +4,7 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 from typing import Optional
+import asyncio
 
 import rclpy
 from rclpy.node import Node
@@ -28,10 +29,14 @@ class CommandLongResult:
 class CommandLongClient(Node):
     """ROS2 client : 對同一個 send_command_long service 發送各種 MAV_CMD_*。"""
 
-    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None:
+    def __init__(self, service_name: str = DEFAULT_SERVICE_NAME, node_name: str = None) -> None:
         if not rclpy.ok():
             rclpy.init(args=None)
-        super().__init__("fc_command_long_client")
+        # 使用提供的 node_name，或使用帶時間戳的預設名稱以避免名稱衝突
+        if node_name is None:
+            import time
+            node_name = f"fc_command_long_client_{int(time.time() * 1000) % 100000}"
+        super().__init__(node_name)
         self._client = self.create_client(MavCommandLong, service_name)
 
     def wait_for_service(self, timeout_sec: float = 3.0) -> bool:
@@ -185,3 +190,192 @@ class CommandLongClient(Node):
             param7=0.0,
             timeout_sec=timeout_sec,
         )
+
+    # ============================================================================
+    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+    # ============================================================================
+
+    # ============================================================================
+    # 【重新實現】正確的非阻塞 async 方法（不使用 ThreadPoolExecutor）
+    # 使用 asyncio 的 polling 機制，避免在線程中調用 spin_until_future_complete
+    # ============================================================================
+
+    async def _send_command_long_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int,
+        command: int,
+        confirmation: int,
+        param1: float,
+        param2: float,
+        param3: float,
+        param4: float,
+        param5: float,
+        param6: float,
+        param7: float,
+        timeout_sec: float,
+    ) -> CommandLongResult:
+        """非阻塞的 async 版本 - 使用 asyncio polling 而非 spin"""
+        req = MavCommandLong.Request()
+        req.target_sysid = target_sysid
+        req.target_compid = target_compid
+        req.command = command
+        req.confirmation = confirmation
+        req.param1 = param1
+        req.param2 = param2
+        req.param3 = param3
+        req.param4 = param4
+        req.param5 = param5
+        req.param6 = param6
+        req.param7 = param7
+        req.timeout_sec = float(timeout_sec)
+
+        # 發送異步調用
+        future = self._client.call_async(req)
+        
+        # 使用 asyncio.sleep 進行 polling，而非 spin_until_future_complete
+        # 使用 10ms 輪詢間隔以匹配 GUI 執行器的 spin_once() 頻率
+        timeout = timeout_sec + 1.0
+        elapsed = 0.0
+        poll_interval = 0.01  # 10ms - 與 GUI timer 頻率一致
+        
+        while elapsed < timeout:
+            if future.done():
+                try:
+                    response = future.result()
+                    if response is None:
+                        return CommandLongResult(
+                            success=False,
+                            message="Service returned None.",
+                            ack_result=-1,
+                        )
+                    return CommandLongResult(
+                        success=response.success,
+                        message=response.message,
+                        ack_result=response.ack_result,
+                    )
+                except Exception as e:
+                    return CommandLongResult(
+                        success=False,
+                        message=f"Error getting result: {e}",
+                        ack_result=-1,
+                    )
+            
+            # 讓出控制權給事件循環，允許 GUI executor 執行
+            await asyncio.sleep(poll_interval)
+            elapsed += poll_interval
+        
+        return CommandLongResult(
+            success=False,
+            message=f"Service timeout after {timeout}s.",
+            ack_result=-1,
+        )
+
+    async def change_mode_async(
+        self,
+        *,
+        target_sysid: int,
+        custom_mode: float,
+        target_compid: int = 0,
+        base_mode: float = 1.0,
+        confirmation: int = 0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 change_mode"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_DO_SET_MODE,
+            confirmation=confirmation,
+            param1=float(base_mode),
+            param2=float(custom_mode),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    async def arm_disarm_async(
+        self,
+        *,
+        target_sysid: int,
+        arm: bool,
+        target_compid: int = 0,
+        confirmation: int = 0,
+        param2: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 arm_disarm"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_COMPONENT_ARM_DISARM,
+            confirmation=confirmation,
+            param1=1.0 if arm else 0.0,
+            param2=float(param2),
+            param3=0.0,
+            param4=0.0,
+            param5=0.0,
+            param6=0.0,
+            param7=0.0,
+            timeout_sec=timeout_sec,
+        )
+
+    async def takeoff_async(
+        self,
+        *,
+        target_sysid: int,
+        altitude_m: float,
+        target_compid: int = 0,
+        min_pitch_deg: float = 0.0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 takeoff"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_TAKEOFF,
+            confirmation=0,
+            param1=float(min_pitch_deg),
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )
+
+    async def land_async(
+        self,
+        *,
+        target_sysid: int,
+        target_compid: int = 0,
+        yaw_deg: float = 0.0,
+        latitude: Optional[float] = None,
+        longitude: Optional[float] = None,
+        altitude_m: float = 0.0,
+        timeout_sec: float = DEFAULT_TIMEOUT_SEC,
+    ) -> CommandLongResult:
+        """非阻塞 async 版本的 land"""
+        return await self._send_command_long_async(
+            target_sysid=target_sysid,
+            target_compid=target_compid,
+            command=COMMAND_NAV_LAND,
+            confirmation=0,
+            param1=0.0,
+            param2=0.0,
+            param3=0.0,
+            param4=float(yaw_deg),
+            param5=float(latitude) if latitude is not None else 0.0,
+            param6=float(longitude) if longitude is not None else 0.0,
+            param7=float(altitude_m),
+            timeout_sec=timeout_sec,
+        )