From 8d65e926343166bde8266e1185f989135c84fac6 Mon Sep 17 00:00:00 2001 From: wenchun Date: Wed, 12 Aug 2026 21:15:35 +0800 Subject: [PATCH] =?UTF-8?q?fix(GUI):=20=E5=85=B1=E7=94=A8=20command/positi?= =?UTF-8?q?on=20client=20=E5=95=9F=E5=8B=95=E9=A0=90=E5=BB=BA=EF=BC=8C?= =?UTF-8?q?=E4=BF=AE=E5=88=87mode=E8=A7=B8=E7=99=BC=E7=9A=84OOM/CPU?= =?UTF-8?q?=E6=9A=B4=E8=A1=9D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 實飛/SITL 對無人機下第一個控制指令（切 mode）時，python GUI 行程 CPU 衝到 112%、記憶體一路爬向 30GB 被 OOM killer 幹掉。 根因（切 mode 是執行期第一個走 get_or_create_client 的動作）： - get_or_create_client / get_or_create_position_client 原本 per-drone 於執行期新建 ROS2 node 並 executor.add_node()。 - CPU：add_node 從 asyncio/Qt 執行緒呼叫，同時 _ros_spin_thread 在另一 執行緒 spin_once 同一個 SingleThreadedExecutor → wait set 損毀 → 空轉。 - 記憶體：執行期新建 node = 新 DDS participant，discovery 撞壞封包 → FastRTPS bad_alloc。解釋了「開機沒事、切 mode 才炸」。 兩個 client 都只打單一 service（send_command_long / pos_global_int）， 靠 request 的 target_sysid 路由、無 per-drone 狀態 → 每台一個節點無意義。 修法：改成「共用節點 + 啟動預建 + 執行期只重用」。 - communication.py: 兩個 per-drone dict → 單一 command_long_client / position_target_client；新增 init_shared_command_clients()； get_or_create_*(drone_id=None) 回傳共用 client（僅留一次性 lazy fallback）。 - gui.py: executor 設好後、spin thread 啟動前於主執行緒預建；cleanup 改 銷毀兩個共用節點。 飛行中不再新建 participant + add_node 不再跨執行緒 → 兩症狀同時消。 SITL 切 mode 實測不再暴衝。 Co-Authored-By: Claude Opus 4.8 --- src/GUI/communication.py | 111 +++++++++++++++++++++------------------ src/GUI/gui.py | 29 +++++----- 2 files changed, 76 insertions(+), 64 deletions(-) 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: