diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 25365fa..d8565bd 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -794,20 +794,19 @@ class DroneMonitor(Node): self.serial_receivers = [] # ================================================================================ - # 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client） + # 共用 command / position client（各一個節點，靠 request 的 target_sysid 路由） # ================================================================================ - # 改為為每個 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 - # ================================================================================ - - # ================================================================================ - # PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto） - # ================================================================================ - self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient} - self.pos_client_counter = 0 + # 兩個 client 都只打到單一 service（send_command_long / pos_global_int）， + # 沒有任何 per-drone 狀態，故不需要 per-drone 節點。改成「啟動時各預建一個、 + # 執行期只重用」，避免： + # (1) 切 mode / 首次 goto 時於執行期新建 ROS2 node = 新 DDS participant， + # 其 discovery handshake 撞到壞掉的外來 participant → FastRTPS bad_alloc → OOM。 + # (2) 從 asyncio/Qt 執行緒對「正在被 _ros_spin_thread spin 的 executor」 + # 跨執行緒 add_node → wait set 損毀 → CPU 空轉 100%。 + self.command_long_client = None # CommandLongClient（共用） + self.position_target_client = None # PositionTargetGlobalIntClient（共用） + self.client_lock = Lock() # 保護共用 client 的建立 + self.executor = None # 將在 gui.py 中設置，用於 add_node # ================================================================================ # 主题检测定时器 @@ -876,57 +875,69 @@ class DroneMonitor(Node): return self.socket_id_mapping[original_socket_id] - def get_or_create_client(self, drone_id): - """為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件""" + def init_shared_command_clients(self): + """啟動時預建共用的 command / position client（各一個節點）。 + + 必須在 _ros_spin_thread 啟動「之前」於主執行緒呼叫：此時 executor 已建立 + 但尚未被 spin，add_node 不會與 spin_once 跨執行緒競爭。之後執行期送指令 + 一律重用這兩個節點，不再於飛行中新建 DDS participant。""" with self.client_lock: - if drone_id not in self.command_long_clients: + if self.command_long_client is None and CommandLongClient is not None: 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 - _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})") - - # 將新 client 添加到主執行器（這樣它的回調才能被處理） + self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared") if self.executor: - self.executor.add_node(client) - _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") - - except TypeError: - # 舊版 CommandLongClient 不支持 node_name 參數，使用預設 - client = CommandLongClient() - self.command_long_clients[drone_id] = client - _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)") - + self.executor.add_node(self.command_long_client) + _log("INFO", "已預建共用 CommandLongClient (node=cmd_long_client_shared)") + except Exception as e: + _log("WARN", f"預建 CommandLongClient 失敗: {e}") + if self.position_target_client is None and PositionTargetGlobalIntClient is not None: + try: + self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared") + if self.executor: + self.executor.add_node(self.position_target_client) + _log("INFO", "已預建共用 PositionTargetGlobalIntClient (node=pos_target_client_shared)") + except Exception as e: + _log("WARN", f"預建 PositionTargetGlobalIntClient 失敗: {e}") + + def get_or_create_client(self, drone_id=None): + """回傳共用的 CommandLongClient（單一節點，靠 request 的 target_sysid 路由）。 + + 正常情況已於啟動時由 init_shared_command_clients() 預建；此處僅保留一次性 + lazy fallback（例如 init 尚未被呼叫），不會在執行期重複新建 participant。 + drone_id 參數保留以相容既有呼叫端，實際不影響路由（路由在 request 內）。""" + if self.command_long_client is not None: + return self.command_long_client + with self.client_lock: + if self.command_long_client is None and CommandLongClient is not None: + try: + self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared") if self.executor: - self.executor.add_node(client) - _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") - + self.executor.add_node(self.command_long_client) + _log("INFO", "已建立共用 CommandLongClient (lazy fallback)") except Exception as e: - _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}") + _log("WARN", f"無法建立共用 CommandLongClient: {e}") return None - return self.command_long_clients[drone_id] + return self.command_long_client + + def get_or_create_position_client(self, drone_id=None): + """回傳共用的 PositionTargetGlobalIntClient（單一節點，靠 target_sysid 路由）。 - def get_or_create_position_client(self, drone_id): - """為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。""" + 同 get_or_create_client：正常已於啟動預建，此處僅一次性 lazy fallback。""" + if self.position_target_client is not None: + return self.position_target_client if PositionTargetGlobalIntClient is None: return None with self.client_lock: - if drone_id not in self.position_target_clients: + if self.position_target_client is None: try: - self.pos_client_counter += 1 - unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}" - client = PositionTargetGlobalIntClient(node_name=unique_name) - self.position_target_clients[drone_id] = client - _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})") + self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared") if self.executor: - self.executor.add_node(client) - _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器") + self.executor.add_node(self.position_target_client) + _log("INFO", "已建立共用 PositionTargetGlobalIntClient (lazy fallback)") except Exception as e: - _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}") + _log("WARN", f"無法建立共用 PositionTargetGlobalIntClient: {e}") return None - return self.position_target_clients[drone_id] + return self.position_target_client def scan_topics(self): topics = self.get_topic_names_and_types() diff --git a/src/GUI/gui.py b/src/GUI/gui.py index aa4ef15..87092b9 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -169,9 +169,14 @@ class ControlStationUI(QMainWindow): self.executor = rclpy.executors.SingleThreadedExecutor() self.executor.add_node(self.monitor) - # 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加 + # 將執行器註冊到 DroneMonitor，以便共用 command/position client 能被添加 self.monitor.executor = self.executor - + + # 啟動時先預建共用 command/position client（必須在 spin thread 啟動前、主執行緒）， + # 避免執行期（切 mode / 首次 goto）新建 DDS participant 觸發 discovery 風暴(OOM) + # 與跨執行緒 add_node race(CPU 100%)。 + self.monitor.init_shared_command_clients() + # 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間 self.ros_thread_running = True self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True) @@ -2781,18 +2786,14 @@ class ControlStationUI(QMainWindow): # Clean up serial receivers for receiver in self.monitor.serial_receivers: receiver.stop() - # Clean up all CommandLongClient instances - for drone_id, client in self.monitor.command_long_clients.items(): - try: - client.destroy_node() - except: - pass - # Clean up all PositionTargetGlobalIntClient instances - for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items(): - try: - client.destroy_node() - except: - pass + # Clean up shared CommandLongClient / PositionTargetGlobalIntClient + for client in (getattr(self.monitor, 'command_long_client', None), + getattr(self.monitor, 'position_target_client', None)): + if client is not None: + try: + client.destroy_node() + except: + pass self.monitor.destroy_node() self.executor.shutdown() except Exception as e: