diff --git a/src/GUI/communication.py b/src/GUI/communication.py index caa480c..4aa904c 100644 --- a/src/GUI/communication.py +++ b/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) @@ -634,7 +650,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 +662,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 +686,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 +697,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}") diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 4784545..a641917 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -1716,6 +1716,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: