Force sync local master to remote ken

src/GUI/communication.py

@@ -419,8 +419,13 @@ class WebSocketMavlinkReceiver(threading.Thread):
 
 class DroneMonitor(Node):
     # Subscribe to drone ROS2 topics
+    _instance = None  # Singleton pattern to prevent duplicate nodes
+    
     def __init__(self):
-        super().__init__('drone_monitor')
+        # Use a unique node name with timestamp to avoid conflicts on restart
+        import time
+        node_name = f'drone_monitor_{int(time.time() * 1000) % 100000}'
+        super().__init__(node_name)
         self.signals = DroneSignals()
         self.drone_topics = {}
         self.lock = Lock()
@@ -461,6 +466,17 @@ class DroneMonitor(Node):
         # ================================================================================
         self.serial_receivers = []
         
+        # ================================================================================
+        # 【新增】初始化 CommandLongClient（持久化，不會每次調用都創建/銷毀）
+        # ================================================================================
+        self.command_long_client = None
+        try:
+            self.command_long_client = CommandLongClient()
+        except Exception as e:
+            print(f"⚠️ 警告: 無法初始化 CommandLongClient: {e}")
+            self.command_long_client = None
+        # ================================================================================
+        
         # 主题检测定时器
         self.create_timer(1.0, self.scan_topics)
     
@@ -586,7 +602,7 @@ class DroneMonitor(Node):
     # ================================================================================
 
     async def set_mode(self, drone_id, mode_name):
-        """使用 CommandLongClient 切換無人機飛行模式"""
+        """使用 CommandLongClient 切換無人機飛行模式（使用非阻塞的 async 方法）"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -603,30 +619,24 @@ 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)
+            if not self.command_long_client:
+                print(f"❌ [SET_MODE] CommandLongClient 未初始化")
+                return False
             
-            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
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.change_mode_async(
+                target_sysid=sysid,
+                custom_mode=float(custom_mode),
+                target_compid=0,
+                base_mode=1.0,
+                timeout_sec=2.0,
+            )
             
-            result = await loop.run_in_executor(executor, _call_set_mode)
-            if result:
+            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}")
@@ -634,7 +644,7 @@ class DroneMonitor(Node):
             return False
 
     async def arm_drone(self, drone_id, arm):
-        """使用 CommandLongClient 執行 ARM/DISARM"""
+        """使用 CommandLongClient 執行 ARM/DISARM（使用非阻塞的 async 方法）"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -646,29 +656,23 @@ class DroneMonitor(Node):
             action_name = "解鎖" if arm else "上鎖"
             print(f"\n📢 [ARM] {drone_id} → {action_name}")
             
-            # 在線程池中調用 CommandLongClient
-            loop = asyncio.get_event_loop()
-            executor = ThreadPoolExecutor(max_workers=1)
+            if not self.command_long_client:
+                print(f"❌ [ARM] CommandLongClient 未初始化")
+                return False
             
-            def _call_arm_disarm():
-                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
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.arm_disarm_async(
+                target_sysid=sysid,
+                arm=arm,
+                target_compid=0,
+                timeout_sec=2.0,
+            )
             
-            result = await loop.run_in_executor(executor, _call_arm_disarm)
-            if result:
+            if result and result.success:
                 print(f"✅ [ARM] {action_name}成功")
                 return True
             else:
-                print(f"❌ [ARM] {action_name}失敗")
+                print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
             print(f"❌ [ARM] 例外錯誤: {e}")
@@ -676,7 +680,7 @@ class DroneMonitor(Node):
             return False
 
     async def takeoff_drone(self, drone_id, altitude):
-        """使用 CommandLongClient 執行無人機起飛"""
+        """使用 CommandLongClient 執行無人機起飛（使用非阻塞的 async 方法）"""
         try:
             # 解析 drone_id 提取 sysid
             parts = drone_id.split('_')
@@ -687,29 +691,23 @@ class DroneMonitor(Node):
             
             print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
             
-            # 在線程池中調用 CommandLongClient
-            loop = asyncio.get_event_loop()
-            executor = ThreadPoolExecutor(max_workers=1)
+            if not self.command_long_client:
+                print(f"❌ [TAKEOFF] CommandLongClient 未初始化")
+                return False
             
-            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
+            # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突）
+            result = await self.command_long_client.takeoff_async(
+                target_sysid=sysid,
+                altitude_m=float(altitude),
+                target_compid=0,
+                timeout_sec=2.0,
+            )
             
-            result = await loop.run_in_executor(executor, _call_takeoff)
-            if result:
+            if result and result.success:
                 print(f"✅ [TAKEOFF] 起飛成功")
                 return True
             else:
-                print(f"❌ [TAKEOFF] 起飛失敗")
+                print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})")
                 return False
         except Exception as e:
             print(f"❌ [TAKEOFF] 例外錯誤: {e}")

src/GUI/gui.py

@@ -33,7 +33,7 @@ from mission_group import (
 # ================================================================================
 
 class ControlStationUI(QMainWindow):
-    VERSION = '2.0.3'
+    VERSION = '2.0.6'
     
     def __init__(self):
         super().__init__()
@@ -52,6 +52,10 @@ 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)
+        
         # 定时处理ROS事件
         self.timer = QTimer()
         self.timer.timeout.connect(self.spin_ros)
@@ -1716,6 +1720,15 @@ class ControlStationUI(QMainWindow):
                 receiver.stop()
             for receiver in self.monitor.ws_receivers:
                 receiver.stop()
+            # Clean up serial receivers
+            for receiver in self.monitor.serial_receivers:
+                receiver.stop()
+            # Clean up CommandLongClient
+            if self.monitor.command_long_client:
+                try:
+                    self.monitor.command_long_client.destroy_node()
+                except:
+                    pass
             self.monitor.destroy_node()
             self.executor.shutdown()
         except Exception as e:

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
@@ -185,3 +186,118 @@ class CommandLongClient(Node):
             param7=0.0,
             timeout_sec=timeout_sec,
         )
+
+    # ============================================================================
+    # 【新增】非阻塞 async 包裝方法（用於 GUI 的非阻塞調用）
+    # 這些方法在 ThreadPoolExecutor 中運行同步版本，以避免阻塞事件循環
+    # ============================================================================
+
+    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（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.change_mode(
+                target_sysid=target_sysid,
+                custom_mode=custom_mode,
+                target_compid=target_compid,
+                base_mode=base_mode,
+                confirmation=confirmation,
+                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（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.arm_disarm(
+                target_sysid=target_sysid,
+                arm=arm,
+                target_compid=target_compid,
+                confirmation=confirmation,
+                param2=param2,
+                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（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.takeoff(
+                target_sysid=target_sysid,
+                altitude_m=altitude_m,
+                target_compid=target_compid,
+                min_pitch_deg=min_pitch_deg,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                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（在 ThreadPoolExecutor 中運行）"""
+        from concurrent.futures import ThreadPoolExecutor
+        loop = asyncio.get_event_loop()
+        executor = ThreadPoolExecutor(max_workers=1)
+        return await loop.run_in_executor(
+            executor,
+            lambda: self.land(
+                target_sysid=target_sysid,
+                target_compid=target_compid,
+                yaw_deg=yaw_deg,
+                latitude=latitude,
+                longitude=longitude,
+                altitude_m=altitude_m,
+                timeout_sec=timeout_sec,
+            )
+        )