chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(GUI): 共用 command/position client 啟動預建，修切mode觸發的OOM/CPU暴衝

Browse Source 
實飛/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 <noreply@anthropic.com>
wenchun
wenchun 1 month ago
parent 4e476f80f6
commit 8d65e92634
2 changed files with 76 additions and 64 deletions
Show Stats Download Patch File Download Diff File

107
src/GUI/communication.py
Unescape Escape View File

@ -794,20 +794,19 @@ class DroneMonitor(Node):
self.serial_receivers = [] self.serial_receivers = []
# ================================================================================ # ================================================================================
# 【新增】初始化 CommandLongClient 字典（為每個 drone 維護獨立的 client） # 共用 command / position client（各一個節點，靠 request 的 target_sysid 路由）
# ================================================================================ # ================================================================================
# 改為為每個 drone 創建獨立的 client，避免多機並行時的競態條件 # 兩個 client 都只打到單一 service（send_command_long / pos_global_int），
self.command_long_clients = {} # {drone_id: CommandLongClient} # 沒有任何 per-drone 狀態，故不需要 per-drone 節點。改成「啟動時各預建一個、
self.client_lock = Lock() # 保護 clients 字典的訪問 # 執行期只重用」，避免：
self.client_counter = 0 # 用於生成唯一的 client 節點名稱 # (1) 切 mode / 首次 goto 時於執行期新建 ROS2 node = 新 DDS participant，
self.executor = None # 將在 gui.py 中設置，用於添加新的 clients # 其 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（共用）
# PositionTargetGlobalIntClient 字典（per-drone，用於 Offboard goto） self.position_target_client = None # PositionTargetGlobalIntClient（共用）
# ================================================================================ self.client_lock = Lock() # 保護共用 client 的建立
self.position_target_clients = {} # {drone_id: PositionTargetGlobalIntClient} self.executor = None # 將在 gui.py 中設置，用於 add_node
self.pos_client_counter = 0
# ================================================================================ # ================================================================================
# 主题检测定时器 # 主题检测定时器
@ -876,57 +875,69 @@ class DroneMonitor(Node):
return self.socket_id_mapping[original_socket_id] return self.socket_id_mapping[original_socket_id]
def get_or_create_client(self, drone_id): def init_shared_command_clients(self):
"""為每個 drone 獲取或創建獨立的 CommandLongClient，避免競態條件""" """啟動時預建共用的 command / position client（各一個節點）。
必須在 _ros_spin_thread 啟動「之前」於主執行緒呼叫：此時 executor 已建立
但尚未被 spin，add_node 不會與 spin_once 跨執行緒競爭。之後執行期送指令
一律重用這兩個節點，不再於飛行中新建 DDS participant。"""
with self.client_lock: 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: try:
# 生成唯一的 client 節點名稱 self.command_long_client = CommandLongClient(node_name="cmd_long_client_shared")
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 添加到主執行器（這樣它的回調才能被處理）
if self.executor: if self.executor:
self.executor.add_node(client) self.executor.add_node(self.command_long_client)
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") _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}")
except TypeError: def get_or_create_client(self, drone_id=None):
# 舊版 CommandLongClient 不支持 node_name 參數，使用預設 """回傳共用的 CommandLongClient（單一節點，靠 request 的 target_sysid 路由）。
client = CommandLongClient()
self.command_long_clients[drone_id] = client
_log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)")
正常情況已於啟動時由 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: if self.executor:
self.executor.add_node(client) self.executor.add_node(self.command_long_client)
_log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") _log("INFO", "已建立共用 CommandLongClient (lazy fallback)")
except Exception as e: except Exception as e:
_log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}") _log("WARN", f"無法建立共用 CommandLongClient: {e}")
return None 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): 同 get_or_create_client：正常已於啟動預建，此處僅一次性 lazy fallback。"""
"""為每個 drone 獲取或創建獨立的 PositionTargetGlobalIntClient。""" if self.position_target_client is not None:
return self.position_target_client
if PositionTargetGlobalIntClient is None: if PositionTargetGlobalIntClient is None:
return None return None
with self.client_lock: with self.client_lock:
if drone_id not in self.position_target_clients: if self.position_target_client is None:
try: try:
self.pos_client_counter += 1 self.position_target_client = PositionTargetGlobalIntClient(node_name="pos_target_client_shared")
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})")
if self.executor: if self.executor:
self.executor.add_node(client) self.executor.add_node(self.position_target_client)
_log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器") _log("INFO", "已建立共用 PositionTargetGlobalIntClient (lazy fallback)")
except Exception as e: except Exception as e:
_log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}") _log("WARN", f"無法建立共用 PositionTargetGlobalIntClient: {e}")
return None return None
return self.position_target_clients[drone_id] return self.position_target_client
def scan_topics(self): def scan_topics(self):
topics = self.get_topic_names_and_types() topics = self.get_topic_names_and_types()

27
src/GUI/gui.py
Unescape Escape View File

@ -169,9 +169,14 @@ class ControlStationUI(QMainWindow):
self.executor = rclpy.executors.SingleThreadedExecutor() self.executor = rclpy.executors.SingleThreadedExecutor()
self.executor.add_node(self.monitor) self.executor.add_node(self.monitor)
# 將執行器註冊到 DroneMonitor，以便動態創建的 CommandLongClient 能被添加 # 將執行器註冊到 DroneMonitor，以便共用 command/position client 能被添加
self.monitor.executor = self.executor 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 主執行緒時間 # 在背景執行緒處理 ROS2 spin，避免佔用 Qt 主執行緒時間
self.ros_thread_running = True self.ros_thread_running = True
self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True) self.ros_thread = threading.Thread(target=self._ros_spin_thread, daemon=True)
@ -2781,18 +2786,14 @@ class ControlStationUI(QMainWindow):
# Clean up serial receivers # Clean up serial receivers
for receiver in self.monitor.serial_receivers: for receiver in self.monitor.serial_receivers:
receiver.stop() receiver.stop()
# Clean up all CommandLongClient instances # Clean up shared CommandLongClient / PositionTargetGlobalIntClient
for drone_id, client in self.monitor.command_long_clients.items(): for client in (getattr(self.monitor, 'command_long_client', None),
try: getattr(self.monitor, 'position_target_client', None)):
client.destroy_node() if client is not None:
except: try:
pass client.destroy_node()
# Clean up all PositionTargetGlobalIntClient instances except:
for drone_id, client in getattr(self.monitor, 'position_target_clients', {}).items(): pass
try:
client.destroy_node()
except:
pass
self.monitor.destroy_node() self.monitor.destroy_node()
self.executor.shutdown() self.executor.shutdown()
except Exception as e: except Exception as e:

Write Preview
Loading…
Cancel
Save