wenchun/AirTrapMine
1
0
Fork 0
forked from chiyu1468/AirTrapMine
Code Pull Requests Activity

Update GUI 2.0.6 components and longCommand

Browse Source
This commit is contained in:
ken910606
2026-04-14 15:09:25 +08:00
parent fc2c642d6c
commit fc00ecb762
4 changed files with 227 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/communication.py
+56 -19
View File
@@ -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:
src/GUI/gui.py
+24 -6
View File
@@ -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()
src/GUI/mission_group.py
+8 -4
View File
@@ -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):
"""根據當前任務類型，顯示/隱藏對應的參數列"""
src/fc_network_apps/longCommand.py
+139 -61
View File
@@ -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,
)