From fc00ecb762add00834f563dfb0afa15609465f98 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 14 Apr 2026 15:09:25 +0800 Subject: [PATCH] Update GUI 2.0.6 components and longCommand --- src/GUI/communication.py | 75 ++++++++--- src/GUI/gui.py | 30 ++++- src/GUI/mission_group.py | 12 +- src/fc_network_apps/longCommand.py | 200 ++++++++++++++++++++--------- 4 files changed, 227 insertions(+), 90 deletions(-) diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 250f5eb..a36810c 100644 --- a/src/GUI/communication.py +++ b/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+)') @@ -619,17 +650,19 @@ class DroneMonitor(Node): print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})") - if not self.command_long_client: - print(f"❌ [SET_MODE] CommandLongClient 未初始化") + # 獲取或創建該 drone 專用的 client（避免多機並行時的競態條件） + client = self.get_or_create_client(drone_id) + if not client: + print(f"❌ [SET_MODE] CommandLongClient 無法初始化") return False # 直接調用 async 方法，無需線程池（避免嵌套執行器衝突） - result = await self.command_long_client.change_mode_async( + result = await client.change_mode_async( target_sysid=sysid, custom_mode=float(custom_mode), target_compid=0, base_mode=1.0, - timeout_sec=2.0, + timeout_sec=5.0, # 增加超時時間以提高多機操作的可靠性 ) if result and result.success: @@ -656,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: @@ -691,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: diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 4d9bf37..ef33e85 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -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() @@ -587,6 +586,9 @@ class ControlStationUI(QMainWindow): self.group_tab_widget.setCurrentIndex(idx) 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 並同步地圖模式""" @@ -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 @@ -961,6 +968,17 @@ class ControlStationUI(QMainWindow): self.active_group_id = None 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): """群組任務完成回呼""" @@ -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() diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index 60b4b8b..38f4322 100644 --- a/src/GUI/mission_group.py +++ b/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() @@ -474,6 +474,10 @@ class GroupPanel(QWidget): def set_all_select_state(self, is_selected): """外部設置全選狀態（按鈕文本保持「全選/取消」）""" self._is_all_selected = is_selected + + def set_delete_enabled(self, enabled): + """啟用或禁用刪除群組按鈕""" + self.delete_group_btn.setEnabled(enabled) def _update_param_visibility(self, _=None): """根據當前任務類型，顯示/隱藏對應的參數列""" diff --git a/src/fc_network_apps/longCommand.py b/src/fc_network_apps/longCommand.py index c4bd8a5..7ebd4bb 100644 --- a/src/fc_network_apps/longCommand.py +++ b/src/fc_network_apps/longCommand.py @@ -29,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: @@ -192,6 +196,83 @@ class CommandLongClient(Node): # 這些方法在 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, *, @@ -202,20 +283,20 @@ class CommandLongClient(Node): 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 版本的 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( @@ -228,20 +309,20 @@ class CommandLongClient(Node): 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 版本的 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( @@ -256,22 +337,20 @@ class CommandLongClient(Node): 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 版本的 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( @@ -285,19 +364,18 @@ class CommandLongClient(Node): 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, - ) + """非阻塞 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, )