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

Merge branch 'master' of http://140.120.108.238:49308/chiyu1468/AirTrapMine

Browse Source 
主專案整合
This commit is contained in:
chiyu1468
2026-04-14 11:11:42 +08:00
parent 2990de2f3f f34693a400
commit b0f1bd56f5
3 changed files with 709 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GUI/communication.py
+172 -32
View File
@@ -4,10 +4,14 @@ import math
import re
import threading
from threading import Lock
from concurrent.futures import ThreadPoolExecutor
import asyncio
import websockets
import json
import socket
import sys
import os
import traceback
from pymavlink import mavutil
from geometry_msgs.msg import Point, Vector3
from sensor_msgs.msg import BatteryState, NavSatFix, Imu
@@ -15,6 +19,23 @@ from std_msgs.msg import Float64, String
from mavros_msgs.msg import State, VfrHud
from mavros_msgs.srv import CommandBool, CommandTOL
# 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入）
_src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
if _src_path not in sys.path:
sys.path.insert(0, _src_path)
# 導入 fc_network_apps 的 longCommand（統一的 MAV_CMD_* API）
try:
from fc_network_apps.longCommand import CommandLongClient
except ImportError as e:
import traceback
print(f"⚠️ 警告: 無法導入 CommandLongClient")
print(f" 错误: {e}")
print(f" 这通常表示 ROS2 的 fc_interfaces 包未被编译或未正确安装")
print(f" 完整堆栈跟踪:")
traceback.print_exc()
CommandLongClient = None
class DroneSignals(QObject):
update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data)
@@ -536,44 +557,163 @@ class DroneMonitor(Node):
setattr(self, f'drone_{sys_id}_subs', subs)
async def arm_drone(self, drone_id, arm):
if drone_id not in self.arm_clients:
return False
client = self.arm_clients[drone_id]
if not client.wait_for_service(timeout_sec=1.0):
return False
request = CommandBool.Request()
request.value = arm
future = client.call_async(request)
# ================================================================================
# 【新增】模式名稱到 custom_mode 值的映射（基於 Copter 模式）
# ================================================================================
MODE_MAPPING = {
"STABILIZE": 0,
"ACRO": 1,
"ALT_HOLD": 2,
"AUTO": 3,
"GUIDED": 4,
"LOITER": 5,
"RTL": 6,
"CIRCLE": 7,
"POSITION": 8,
"LAND": 9,
"OF_LOITER": 10,
"DRIFT": 11,
"SPORT": 13,
"FLIP": 14,
"AUTOTUNE": 15,
"POSHOLD": 16,
"BRAKE": 17,
"THROW": 18,
"AVOID_ADSB": 19,
"GUIDED_NOGPS": 20,
"SMART_RTL": 21,
}
# ================================================================================
async def set_mode(self, drone_id, mode_name):
"""使用 CommandLongClient 切換無人機飛行模式"""
try:
response = await future
return response.success
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
# 獲取模式對應的 custom_mode 值
custom_mode = self.MODE_MAPPING.get(mode_name)
if custom_mode is None:
print(f"❌ [SET_MODE] 未知模式: {mode_name}")
return False
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
# 在線程池中調用 CommandLongClient
loop = asyncio.get_event_loop()
executor = ThreadPoolExecutor(max_workers=1)
def _call_set_mode():
client = CommandLongClient() if CommandLongClient else None
if not client:
return False
result = client.change_mode(
target_sysid=sysid,
custom_mode=float(custom_mode),
target_compid=0,
base_mode=1.0,
timeout_sec=2.0,
)
client.destroy_node()
return result.success if result else False
result = await loop.run_in_executor(executor, _call_set_mode)
if result:
print(f"✅ [SET_MODE] 模式切換成功")
return True
else:
print(f"❌ [SET_MODE] 模式切換失敗")
return False
except Exception as e:
self.get_logger().error(f'Arm service call failed: {e}')
print(f"❌ [SET_MODE] 例外錯誤: {e}")
traceback.print_exc()
return False
async def takeoff_drone(self, drone_id, altitude=10.0):
if drone_id not in self.takeoff_clients:
return False
client = self.takeoff_clients[drone_id]
if not client.wait_for_service(timeout_sec=1.0):
return False
request = CommandTOL.Request()
request.altitude = altitude
request.min_pitch = 0.0
request.yaw = 0.0
future = client.call_async(request)
async def arm_drone(self, drone_id, arm):
"""使用 CommandLongClient 執行 ARM/DISARM"""
try:
response = await future
return response.success
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
action_name = "解鎖" if arm else "上鎖"
print(f"\n📢 [ARM] {drone_id} → {action_name}")
# 在線程池中調用 CommandLongClient
loop = asyncio.get_event_loop()
executor = ThreadPoolExecutor(max_workers=1)
def _call_arm_disarm():
client = CommandLongClient() if CommandLongClient else None
if not client:
return False
result = client.arm_disarm(
target_sysid=sysid,
arm=arm,
target_compid=0,
timeout_sec=2.0,
)
client.destroy_node()
return result.success if result else False
result = await loop.run_in_executor(executor, _call_arm_disarm)
if result:
print(f"✅ [ARM] {action_name}成功")
return True
else:
print(f"❌ [ARM] {action_name}失敗")
return False
except Exception as e:
self.get_logger().error(f'Takeoff service call failed: {e}')
print(f"❌ [ARM] 例外錯誤: {e}")
traceback.print_exc()
return False
async def takeoff_drone(self, drone_id, altitude):
"""使用 CommandLongClient 執行無人機起飛"""
try:
# 解析 drone_id 提取 sysid
parts = drone_id.split('_')
if len(parts) < 2:
print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}")
return False
sysid = int(parts[-1])
print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)")
# 在線程池中調用 CommandLongClient
loop = asyncio.get_event_loop()
executor = ThreadPoolExecutor(max_workers=1)
def _call_takeoff():
client = CommandLongClient() if CommandLongClient else None
if not client:
return False
result = client.takeoff(
target_sysid=sysid,
altitude_m=float(altitude),
target_compid=0,
timeout_sec=2.0,
)
client.destroy_node()
return result.success if result else False
result = await loop.run_in_executor(executor, _call_takeoff)
if result:
print(f"✅ [TAKEOFF] 起飛成功")
return True
else:
print(f"❌ [TAKEOFF] 起飛失敗")
return False
except Exception as e:
print(f"❌ [TAKEOFF] 例外錯誤: {e}")
traceback.print_exc()
return False
def send_setpoint(self, drone_id, x, y, z):
src/GUI/gui.py
+491 -54
View File
@@ -12,6 +12,7 @@ import asyncio
import json
import subprocess
import time
import traceback
# 導入分離的類別
from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver
@@ -32,15 +33,18 @@ from mission_group import (
# ================================================================================
class ControlStationUI(QMainWindow):
VERSION = '1.0.1'
VERSION = '2.0.3'
def __init__(self):
super().__init__()
self.setWindowTitle(f'GCS v{self.VERSION}')
self.resize(1400, 900)
# 初始化ROS2
rclpy.init()
# 初始化消息隊列（用於線程安全的 GUI 更新）
import queue
self.message_queue = queue.Queue()
# 初始化ROS2 Monitor（ROS2 本身在 main() 中已初始化）
self.monitor = DroneMonitor()
self.monitor.signals.update_signal.connect(self.update_ui)
@@ -53,11 +57,22 @@ class ControlStationUI(QMainWindow):
self.timer.timeout.connect(self.spin_ros)
self.timer.start(10)
# 驅動 asyncio 事件循環的定時器（新增 - 關鍵！）
# 這允許異步任務（如 arm_drone）能夠執行
self.asyncio_timer = QTimer()
self.asyncio_timer.timeout.connect(self._spin_asyncio)
self.asyncio_timer.start(10) # 每 10ms 驅動一次 asyncio
# 初始化 panel 和 map 更新（10Hz）
self.panel_map_timer = QTimer()
self.panel_map_timer.timeout.connect(self._update_panel_and_map)
self.panel_map_timer.start(100) # 10Hz
# 消息隊列處理定時器（來自線程的 GUI 更新）
self.msg_queue_timer = QTimer()
self.msg_queue_timer.timeout.connect(self._process_message_queue)
self.msg_queue_timer.start(50) # 每 50ms 檢查一次
# 快取消息數據，以便在沒有新消息時使用上一次的值
self._message_cache = {}
@@ -164,20 +179,29 @@ class ControlStationUI(QMainWindow):
# ========== 任務群組 Tab ==========
group_header = QHBoxLayout()
# 標題 + 收起/展開按鈕
title_layout = QHBoxLayout()
group_title = QLabel("任務群組")
group_title.setStyleSheet(
"color: #DDD; font-size: 14px; font-weight: bold; padding: 2px;")
group_header.addWidget(group_title)
group_header.addStretch()
add_group_btn = QPushButton("+ 新增群組")
add_group_btn.setStyleSheet("""
QPushButton { background-color: #4A9EFF; color: white; border: none;
padding: 5px 12px; border-radius: 4px; font-size: 12px; font-weight: bold; }
QPushButton:hover { background-color: #3A8EEF; }
title_layout.addWidget(group_title)
# 收起/展開按鈕
self.toggle_group_btn = QPushButton("▼")
self.toggle_group_btn.setStyleSheet("""
QPushButton { background-color: #555; color: white; border: none;
padding: 3px 8px; border-radius: 3px; font-size: 12px; font-weight: bold;
min-width: 30px; max-width: 30px; }
QPushButton:hover { background-color: #666; }
""")
add_group_btn.clicked.connect(self._add_mission_group)
group_header.addWidget(add_group_btn)
self.toggle_group_btn.setToolTip("收起/展開任務群組")
self.toggle_group_btn.clicked.connect(self._toggle_group_panel)
title_layout.addWidget(self.toggle_group_btn)
title_layout.addStretch()
group_header.addLayout(title_layout)
group_header.addStretch()
clear_traj_btn = QPushButton("清除軌跡")
clear_traj_btn.setStyleSheet("""
@@ -204,6 +228,9 @@ class ControlStationUI(QMainWindow):
self.group_tab_widget.setFixedHeight(150)
self.group_tab_widget.currentChanged.connect(self._on_group_tab_changed)
right_layout.addWidget(self.group_tab_widget)
# 🌟 新增：保存群組面板的展開狀態
self.group_panel_expanded = True
# 預設建立 Group A
self._add_mission_group()
@@ -353,6 +380,8 @@ class ControlStationUI(QMainWindow):
status_label.setToolTip("運行中")
self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000)
except Exception as e:
print(f"❌ Serial 連接啟動失敗: {str(e)}")
traceback.print_exc()
self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000)
def remove_serial_connection(self, conn, panel):
@@ -377,22 +406,39 @@ class ControlStationUI(QMainWindow):
# ================================================================================
def handle_mode_change(self, drone_id):
print(f"\n📢 [GUI] handle_mode_change 被调用")
print(f" drone_id: {drone_id}")
# 從 active group 的 mode_combo 讀取模式
group = self._get_active_group()
if group:
panel = self.group_panels.get(group.group_id)
mode = panel.mode_combo.currentText() if panel else "GUIDED"
print(f" 从群组读取模式: {mode}")
else:
mode = "GUIDED"
print(f" 没有活跃群组，使用默认模式: {mode}")
loop = asyncio.get_event_loop()
future = self.monitor.set_mode(drone_id, mode)
loop.create_task(self.handle_service_response(future, f"切換模式 {mode} {drone_id}"))
def handle_arm(self, drone_id):
print(f"\n📢 [GUI] handle_arm 被調用")
print(f" drone_id: {drone_id}")
loop = asyncio.get_event_loop()
print(f" 獲得事件循環: {loop}")
arm_state = not self.monitor.get_arm_state(drone_id)
future = self.monitor.arm_drone(drone_id, arm_state)
loop.create_task(self.handle_service_response(future, f"{'解鎖' if arm_state else '上鎖'} {drone_id}"))
print(f" 當前鎖定狀態: {not arm_state}, 目標狀態: {arm_state}")
print(f" 準備調用 arm_drone(drone_id={drone_id}, arm={arm_state})")
coro = self.monitor.arm_drone(drone_id, arm_state)
print(f" arm_drone() 返回類型: {type(coro)}")
print(f" coroutine 對象: {coro}")
action_text = f"{'解鎖' if arm_state else '上鎖'} {drone_id}"
print(f" 準備提交到事件循環: {action_text}")
task = loop.create_task(self.handle_service_response(coro, action_text))
print(f" task 已創建: {task}")
print(f" 已提交到事件循環\n")
def handle_takeoff(self, drone_id):
loop = asyncio.get_event_loop()
@@ -425,20 +471,52 @@ class ControlStationUI(QMainWindow):
self.statusBar().showMessage("座標格式錯誤", 3000)
async def handle_service_response(self, future, action):
print(f"\n📥 [GUI] handle_service_response 開始處理: {action}")
print(f" Future/Coroutine 類型: {type(future)}")
print(f" Future/Coroutine 對象: {future}")
try:
print(f" ├─ 等待 future/coroutine 完成...")
print(f" └─ (這是一個阻塞等待，直到服務返回)\n")
result = await future
print(f"\n ✓ Future/Coroutine 完成，接收到返回值！")
print(f" ├─ 返回值類型: {type(result)}")
print(f" ├─ 返回值內容: {result}")
print(f" ├─ 返回值 == True: {result == True}")
print(f" ├─ 返回值 is True: {result is True}")
print(f" └─ bool(返回值): {bool(result)}")
# 詳細檢查 result 結構（用於調試 fc_network 回傳值）
if hasattr(result, '__dict__'):
print(f" └─ 返回值屬性: {result.__dict__}")
if result:
print(f"\n✅ {action} 成功 (result={result})")
self.statusBar().showMessage(f"{action} 成功", 3000)
else:
print(f"\n❌ {action} 失敗 (result={result})")
self.statusBar().showMessage(f"{action} 失敗", 3000)
except asyncio.CancelledError:
print(f"⚠️ {action} 被取消")
except Exception as e:
print(f"❌ {action} 錯誤: {str(e)}")
traceback.print_exc()
self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000)
def handle_arm_selected(self):
print(f"\n📢 [GUI] handle_arm_selected 被調用")
selected = list(self.monitor.selected_drones)
print(f" 已選擇的無人機: {selected}")
loop = asyncio.get_event_loop()
for drone_id in self.monitor.selected_drones:
future = self.monitor.arm_drone(drone_id, True)
loop.create_task(self.handle_service_response(future, f"批次解鎖 {drone_id}"))
for drone_id in selected:
print(f" 準備批次解鎖: {drone_id}")
coro = self.monitor.arm_drone(drone_id, True)
print(f" arm_drone 返回: {coro}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行
asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, f"批次解鎖 {drone_id}"),
loop
)
print(f" handle_arm_selected 完成\n")
def handle_takeoff_selected(self):
loop = asyncio.get_event_loop()
@@ -456,6 +534,23 @@ class ControlStationUI(QMainWindow):
self._group_counter += 1
return gid
def _toggle_group_panel(self):
"""🌟 收起/展開任務群組面板"""
if self.group_panel_expanded:
# 收起
self.group_tab_widget.setFixedHeight(0)
self.group_tab_widget.hide()
self.toggle_group_btn.setText("▶")
self.toggle_group_btn.setToolTip("展開任務群組")
self.group_panel_expanded = False
else:
# 展開
self.group_tab_widget.setFixedHeight(150)
self.group_tab_widget.show()
self.toggle_group_btn.setText("▼")
self.toggle_group_btn.setToolTip("收起任務群組")
self.group_panel_expanded = True
def _add_mission_group(self):
"""新增一個任務群組"""
gid = self._next_group_id()
@@ -475,6 +570,8 @@ class ControlStationUI(QMainWindow):
panel.box_select_requested.connect(self._handle_box_select)
panel.select_all_requested.connect(self._handle_select_all_for_group)
panel.clear_group_requested.connect(self._handle_clear_group)
panel.add_group_requested.connect(self._add_mission_group)
panel.delete_group_requested.connect(self._handle_delete_group)
self.group_panels[gid] = panel
# 用帶顏色的 tab 標題
@@ -538,6 +635,19 @@ class ControlStationUI(QMainWindow):
if panel:
panel.update_drone_list()
panel.update_status()
# 同步更新左側面板的 checkbox 狀態
self.monitor.selected_drones = group.drone_ids.copy()
for drone_id in all_ids:
if drone_id in self.drones:
checkbox = self.drones[drone_id].get_checkbox()
if checkbox:
checkbox.blockSignals(True)
checkbox.setChecked(drone_id in group.drone_ids)
checkbox.blockSignals(False)
# 更新 socket 群組的 checkbox 狀態
self.update_group_checkbox_state(self.get_socket_id(drone_id))
self.statusBar().showMessage(
f"Group {group_id}: 已分配 {len(group.drone_ids)} 台無人機", 3000)
@@ -611,23 +721,86 @@ class ControlStationUI(QMainWindow):
def _handle_group_mode_change(self, group_id, mode):
"""切換群組內所有無人機的飛行模式"""
print(f"\n📢 [GUI] _handle_group_mode_change 被调用", flush=True)
print(f" group_id: {group_id}, mode: {mode}", flush=True)
group = self.mission_groups.get(group_id)
if not group:
print(f"❌ 找不到群組: {group_id}", flush=True)
return
if not group.drone_ids:
print(f"⚠️ 群組中沒有無人機", flush=True)
self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000)
return
print(f" 準備為 {len(group.drone_ids)} 台無人機切換模式...", flush=True)
self.statusBar().showMessage(f"正在切換模式...", 1000)
# 使用 asyncio 執行（通過事件循環）
async def do_mode_changes_async():
print(f"\n 【異步任務】開始執行模式切換", flush=True)
for drone_id in group.drone_ids:
print(f"\n 【切換】{drone_id} → {mode}", flush=True)
try:
result = await self.monitor.set_mode(drone_id, mode)
if result:
msg = f"✅ {drone_id} 切換成功"
print(f" {msg}", flush=True)
self.message_queue.put((msg, 2000))
else:
msg = f"❌ {drone_id} 切換失敗"
print(f" {msg}", flush=True)
self.message_queue.put((msg, 2000))
except Exception as e:
msg = f"❌ {drone_id} 錯誤: {str(e)}"
print(f" {msg}", flush=True)
traceback.print_exc()
self.message_queue.put((msg, 2000))
print(f"\n 【完成】模式切換任務結束\n", flush=True)
# 通過事件循環提交異步任務
print(f" 【排隊】將任務提交至事件循環", flush=True)
loop = asyncio.get_event_loop()
for drone_id in group.drone_ids:
future = self.monitor.set_mode(drone_id, mode)
loop.create_task(self.handle_service_response(future, f"{drone_id} 切換 {mode}"))
asyncio.run_coroutine_threadsafe(
do_mode_changes_async(),
loop
)
def _handle_group_arm(self, group_id):
"""解鎖群組內所有無人機"""
print(f"\n📢 [GUI] _handle_group_arm 被調用")
print(f" 群組 ID: {group_id}")
group = self.mission_groups.get(group_id)
print(f" 群組存在: {group is not None}")
if not group:
print(f" ⚠️ 群組不存在，返回\n")
return
print(f" 群組內無人機: {group.drone_ids}")
loop = asyncio.get_event_loop()
print(f" 事件循環: {loop}")
for drone_id in group.drone_ids:
future = self.monitor.arm_drone(drone_id, True)
loop.create_task(self.handle_service_response(future, f"解鎖 {drone_id}"))
print(f"\n ┌─ 處理無人機: {drone_id}")
print(f" ├─ 準備調用 arm_drone(drone_id={drone_id}, arm=True)")
coro = self.monitor.arm_drone(drone_id, True)
print(f" ├─ arm_drone 返回類型: {type(coro)}")
action_text = f"解鎖 {drone_id}"
print(f" ├─ 準備提交到事件循環: {action_text}")
# 使用 run_coroutine_threadsafe() 正確地在事件循環中運行 coroutine
# 這是 Qt + asyncio 整合的正確方式
future = asyncio.run_coroutine_threadsafe(
self.handle_service_response(coro, action_text),
loop
)
print(f" ├─ Future 已創建: {future}")
print(f" └─ Future 將在事件循環中執行")
print(f"\n _handle_group_arm 完成（coroutine 已提交至事件循環）\n")
def _handle_group_takeoff(self, group_id, altitude):
"""群組內所有無人機起飛"""
@@ -663,23 +836,59 @@ class ControlStationUI(QMainWindow):
if panel:
panel.update_drone_list()
panel.update_status()
# 同步更新左側面板的 checkbox 狀態
self.monitor.selected_drones = group.drone_ids.copy()
for drone_id in self.drones.keys():
checkbox = self.drones[drone_id].get_checkbox()
if checkbox:
checkbox.blockSignals(True)
checkbox.setChecked(drone_id in group.drone_ids)
checkbox.blockSignals(False)
self.update_group_checkbox_state(self.get_socket_id(drone_id))
self.statusBar().showMessage(
f"Group {group_id}: 框選分配 {len(valid)} 台無人機", 3000)
def _handle_select_all_for_group(self, group_id):
"""全選所有可用無人機，直接分配到該群組"""
"""全選/取消全選 - Toggle 邏輯"""
group = self.mission_groups.get(group_id)
if not group:
return
other = self._get_other_assigned(group_id)
available = {did for did in self.drones.keys() if did not in other}
group.drone_ids = available
# Toggle 邏輯：如果已全選，則清空；否則全選
if group.drone_ids == available:
# 已全選 → 清空
group.drone_ids = set()
self.monitor.selected_drones.clear()
msg_status = "已取消全選"
else:
# 未全選 → 全選
group.drone_ids = available
self.monitor.selected_drones = group.drone_ids.copy()
msg_status = f"全選分配 {len(available)} 台無人機"
panel = self.group_panels.get(group_id)
if panel:
panel.update_drone_list()
panel.update_status()
# 更新按鈕文本
panel.set_all_select_state(group.drone_ids == available)
# 同步更新左側面板的 checkbox 狀態
for drone_id in self.drones.keys():
checkbox = self.drones[drone_id].get_checkbox()
if checkbox:
checkbox.blockSignals(True)
checkbox.setChecked(drone_id in group.drone_ids)
checkbox.blockSignals(False)
self.update_group_checkbox_state(self.get_socket_id(drone_id))
self.statusBar().showMessage(
f"Group {group_id}: 全選分配 {len(available)} 台無人機", 3000)
f"Group {group_id}: {msg_status}", 3000)
def _handle_clear_group(self, group_id):
"""清除群組的無人機分配"""
@@ -696,9 +905,59 @@ class ControlStationUI(QMainWindow):
panel.update_drone_list()
panel.update_status()
panel.clear_mission_info()
# 同步更新左側面板的 checkbox 狀態（全部取消勾選）
self.monitor.selected_drones.clear()
for drone_id in self.drones.keys():
checkbox = self.drones[drone_id].get_checkbox()
if checkbox:
checkbox.blockSignals(True)
checkbox.setChecked(False)
checkbox.blockSignals(False)
self.update_group_checkbox_state(self.get_socket_id(drone_id))
self.statusBar().showMessage(
f"Group {group_id}: 已清除分組", 3000)
def _handle_delete_group(self, group_id):
"""刪除一個任務群組"""
if group_id not in self.mission_groups:
self.statusBar().showMessage(f"Group {group_id} 不存在", 3000)
return
# 停止群組的執行（如果有）
group = self.mission_groups[group_id]
if group.executor:
group.executor.stop()
# 移除地圖上的任務計畫
self.drone_map.clear_mission_plan_for_group(group_id)
# 移除 tab
for i in range(self.group_tab_widget.count()):
if f"Group {group_id}" in self.group_tab_widget.tabText(i):
self.group_tab_widget.removeTab(i)
break
# 移除資料
del self.mission_groups[group_id]
if group_id in self.group_panels:
del self.group_panels[group_id]
# 更新 active group
if self.active_group_id == group_id:
if self.group_tab_widget.count() > 0:
self.group_tab_widget.setCurrentIndex(0)
# 更新 active_group_id 為當前 tab 的群組
for gid, panel in self.group_panels.items():
if panel == self.group_tab_widget.currentWidget().widget():
self.active_group_id = gid
break
else:
self.active_group_id = None
self.statusBar().showMessage(f"已刪除 Group {group_id}", 3000)
def _on_group_mission_completed(self, group_id):
"""群組任務完成回呼"""
panel = self.group_panels.get(group_id)
@@ -739,23 +998,111 @@ class ControlStationUI(QMainWindow):
# ================================================================================
def handle_group_selection(self, socket_id, state):
"""處理 socket 群組 checkbox 的勾選/取消勾選
這個方法在用戶點擊 socket 群組的 checkbox 時被調用。
需要同時更新：
1. 該 socket 下所有無人機的 checkbox
2. self.monitor.selected_drones（用於控制面板同步）
3. 右侧活躍群組的無人機分配（新增）
參數：
socket_id: socket ID (str)
state: Qt.CheckState 的整數值 (0=Unchecked, 1=PartiallyChecked, 2=Checked)
"""
print(f"\n📢 [GUI] handle_group_selection 被調用", flush=True)
print(f" socket_id: {socket_id}, state: {state}", flush=True)
print(f" state 類型: {type(state)}", flush=True)
# 獲取該 socket 下所有無人機
group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
is_checked = state == Qt.CheckState.Checked.value
print(f" 該 socket 下的無人機: {group_drones}", flush=True)
# 判斷是否勾選（只有 state == 2 時才是 Checked）
is_checked = (state == 2) # Qt.CheckState.Checked.value == 2
print(f" is_checked: {is_checked}", flush=True)
# 更新該 socket 下所有無人機的 checkbox 狀態
for drone_id in group_drones:
checkbox = self.drones[drone_id].get_checkbox()
if checkbox:
print(f" └─ 更新 {drone_id}: setChecked({is_checked})", flush=True)
checkbox.blockSignals(True)
checkbox.setChecked(is_checked)
checkbox.blockSignals(False)
if is_checked: self.monitor.selected_drones.add(drone_id)
else: self.monitor.selected_drones.discard(drone_id)
# 同時更新 monitor.selected_drones 以同步控制面板
if is_checked:
self.monitor.selected_drones.add(drone_id)
else:
self.monitor.selected_drones.discard(drone_id)
# 👇 新增：同步更新右侧活躍群組的無人機分配
if self.active_group_id:
group = self.mission_groups.get(self.active_group_id)
panel = self.group_panels.get(self.active_group_id)
if group and panel:
print(f" ├─ 同步右侧群組 {self.active_group_id}", flush=True)
if is_checked:
# 勾選時：將該 socket 下的無人機添加到活躍群組
for drone_id in group_drones:
group.drone_ids.add(drone_id)
print(f" │ └─ 添加到群組: {group_drones}", flush=True)
else:
# 取消勾選時：從活躍群組移除該 socket 下的無人機
for drone_id in group_drones:
group.drone_ids.discard(drone_id)
print(f" │ └─ 從群組移除: {group_drones}", flush=True)
# 更新右側群組面板的顯示
panel.update_drone_list()
panel.update_status()
print(f" │ └─ 已更新右侧群組面板", flush=True)
print(f" 最終 selected_drones: {self.monitor.selected_drones}", flush=True)
print(f"✓ handle_group_selection 完成\n", flush=True)
def handle_drone_selection(self, drone_id, state):
if state == Qt.CheckState.Checked.value:
is_checked = state == Qt.CheckState.Checked.value
if is_checked:
self.monitor.selected_drones.add(drone_id)
else:
self.monitor.selected_drones.discard(drone_id)
self.update_group_checkbox_state(self.get_socket_id(drone_id))
# 同步更新任務群組的無人機分配狀態
# 遍歷所有任務群組，更新已分配的無人機列表顯示
if not is_checked:
# 取消勾選時：從所有包含該無人機的群組中移除
for group_id, group in self.mission_groups.items():
if drone_id in group.drone_ids:
group.drone_ids.discard(drone_id)
panel = self.group_panels.get(group_id)
if panel:
panel.update_drone_list()
panel.update_status()
# 更新全選按鈕狀態
other = self._get_other_assigned(group_id)
available = {did for did in self.drones.keys() if did not in other}
panel.set_all_select_state(group.drone_ids == available)
else:
# 勾選時：檢查該無人機是否已分配給其他群組，若未分配則添加到當前活躍群組
is_already_assigned = any(
drone_id in group.drone_ids
for group in self.mission_groups.values()
)
if not is_already_assigned and self.active_group_id:
# 無人機未被分配給任何群組，可以添加到當前活躍群組
group = self.mission_groups.get(self.active_group_id)
panel = self.group_panels.get(self.active_group_id)
if group and panel:
group.drone_ids.add(drone_id)
panel.update_drone_list()
panel.update_status()
# 更新全選按鈕狀態
other = self._get_other_assigned(self.active_group_id)
available = {did for did in self.drones.keys() if did not in other}
panel.set_all_select_state(group.drone_ids == available)
def update_group_checkbox_state(self, socket_id):
group_drones = [did for did in self.drones.keys() if self.get_socket_id(did) == socket_id]
@@ -890,7 +1237,6 @@ class ControlStationUI(QMainWindow):
)
except Exception as e:
self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000)
import traceback
traceback.print_exc()
# ================================================================================
@@ -962,7 +1308,6 @@ class ControlStationUI(QMainWindow):
)
except Exception as e:
self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000)
import traceback
traceback.print_exc()
# ================================================================================
@@ -1042,7 +1387,6 @@ class ControlStationUI(QMainWindow):
)
except Exception as e:
self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000)
import traceback
traceback.print_exc()
# ================================================================================
@@ -1180,7 +1524,6 @@ class ControlStationUI(QMainWindow):
self._map_update_count += 1
now = time.time()
if now - self._map_update_time >= 1.0:
print(f"[Panel/Map Update] {self._map_update_count} Hz")
self._map_update_time = now
self._map_update_count = 0
@@ -1314,32 +1657,126 @@ class ControlStationUI(QMainWindow):
def _process_message_queue(self):
"""處理來自後台線程的消息隊列（更新 GUI 狀態欄）"""
try:
while True:
try:
message, duration = self.message_queue.get_nowait()
self.statusBar().showMessage(message, duration)
except:
break
except Exception as e:
print(f"❌ 消息隊列處理錯誤: {e}", flush=True)
traceback.print_exc()
def _spin_asyncio(self):
"""驅動 asyncio 事件循環，允許異步任務執行
關鍵修復：asyncio 事件循環不會自動運行。
這個定時器會定期執行事件循環，讓 run_coroutine_threadsafe() 提交的任務能夠執行。
"""
try:
loop = asyncio.get_event_loop()
if loop and not loop.is_closed() and not loop.is_running():
# 執行事件循環直到沒有掛起的任務或超時
# 使用 run_until_complete() 配合一個快速返回的 coroutine
loop.run_until_complete(asyncio.sleep(0))
except Exception as e:
# 靜默忽略任何錯誤，防止 Qt 定時器出現異常
# 但仍然打印詳細的堆棧跟踪以便調試
traceback.print_exc()
def spin_ros(self):
try:
self.executor.spin_once(timeout_sec=0.01)
for (drone_id, msg_type), data in self.monitor.latest_data.items():
self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
self.monitor.latest_data.clear()
# 仅在 ROS2 正常工作时才尝试 spin
if rclpy.ok():
self.executor.spin_once(timeout_sec=0.01)
for (drone_id, msg_type), data in self.monitor.latest_data.items():
self.monitor.signals.update_signal.emit(msg_type, drone_id, data)
self.monitor.latest_data.clear()
except RuntimeError as e:
# ROS2 context 被破坏或不可用
if "Context" in str(e) or "context" in str(e).lower():
print(f"⚠️ ROS2 context 錯誤（忽略）: {e}", flush=True)
else:
print(f"ROS spin error: {e}", flush=True)
traceback.print_exc()
except Exception as e:
print(f"ROS spin error: {e}")
print(f"ROS spin error: {e}", flush=True)
traceback.print_exc()
def closeEvent(self, event):
for group in self.mission_groups.values():
if group.executor:
group.executor.stop()
self.command_sender.close()
for receiver in self.udp_receivers:
receiver.stop()
for receiver in self.monitor.ws_receivers:
receiver.stop()
self.monitor.destroy_node()
self.executor.shutdown()
rclpy.shutdown()
try:
for group in self.mission_groups.values():
if group.executor:
group.executor.stop()
self.command_sender.close()
for receiver in self.udp_receivers:
receiver.stop()
for receiver in self.monitor.ws_receivers:
receiver.stop()
self.monitor.destroy_node()
self.executor.shutdown()
except Exception as e:
print(f"⚠️ 清理資源時出錯: {e}", flush=True)
traceback.print_exc()
# 安全地 shutdown ROS2
try:
if rclpy.ok():
rclpy.shutdown()
except RuntimeError as e:
print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
traceback.print_exc()
event.accept()
def main():
"""
GUI 應用程式的主入口點
標準 ROS2 + Qt 架構：
1. 在最外層/最前面只做一次 rclpy.init()
2. 啟動 Qt 應用程式
3. 在 finally 中做 rclpy.shutdown()
這樣可以確保所有 ROS2 相關操作都共享同一個初始化狀態
"""
# 第一步：在最外層只初始化一次 ROS2（終極防護）
# 添加 rclpy.ok() 檢查，防止重複初始化導致 "Context.init() must only be called once" 錯誤
print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True)
if not rclpy.ok():
print("🚀 [GUI 主程式] ROS2 未初始化，開始初始化...", flush=True)
rclpy.init()
print("✅ [GUI 主程式] ROS2 已全局初始化（由 GUI 主程式霸佔）", flush=True)
else:
print("ℹ️ [GUI 主程式] ROS2 已初始化，跳過重複初始化", flush=True)
try:
# 第二步：啟動 Qt 應用程式
print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True)
app = QApplication(sys.argv)
station = ControlStationUI()
station.show()
print("✓ [GUI 主程式] 應用程式已啟動", flush=True)
# 第三步：進入 Qt 事件循環（阻塞直到用戶關閉應用）
print("🎯 [GUI 主程式] 進入主事件循環，等待用戶操作...", flush=True)
sys.exit(app.exec())
finally:
# 第四步：只有當 GUI 視窗被關閉時，才做 ROS2 cleanup（終極防護）
# 這裡確保 ROS2 被正確且安全地關閉
print("\n🛑 [GUI 主程式] 應用程式關閉，正在清理 ROS2 資源...", flush=True)
if rclpy.ok():
rclpy.shutdown()
print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True)
else:
print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用，無需重複 shutdown", flush=True)
print("✓ [GUI 主程式] 應用程式完全退出", flush=True)
if __name__ == '__main__':
app = QApplication(sys.argv)
station = ControlStationUI()
station.show()
app.exec()
main()
src/GUI/mission_group.py
+46 -22
View File
@@ -146,6 +146,8 @@ class GroupPanel(QWidget):
box_select_requested = pyqtSignal(str) # group_id — 框選直接分配
select_all_requested = pyqtSignal(str) # group_id — 全選直接分配
clear_group_requested = pyqtSignal(str) # group_id — 清除分組
add_group_requested = pyqtSignal() # 新增群組
delete_group_requested = pyqtSignal(str) # group_id — 刪除群組
BUTTON_STYLE = """
QPushButton {{ background-color: {bg}; color: {fg}; border: none;
@@ -157,6 +159,8 @@ class GroupPanel(QWidget):
def __init__(self, group: MissionGroup, parent=None):
super().__init__(parent)
self.group = group
self._is_all_selected = False # 追蹤全選狀態
self.all_btn_ref = None # 保存全選按鈕的參考
self._build_ui()
def _make_sep(self):
@@ -296,7 +300,7 @@ class GroupPanel(QWidget):
mid.addLayout(mid_body)
# ============================
# 選取與分組（2x2 按鈕）
# 選取與分組（3x2 按鈕）
# ============================
right = QVBoxLayout()
right.setSpacing(3)
@@ -310,9 +314,28 @@ class GroupPanel(QWidget):
self.drone_list_label.setWordWrap(True)
right.addWidget(self.drone_list_label)
# 2x2 按鈕網格
# 3x2 按鈕網格：第一行 框選 全選 新增群組
grid_r1 = QHBoxLayout()
grid_r1.setSpacing(3)
box_btn = QPushButton("框選")
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
box_btn.clicked.connect(
lambda: self.box_select_requested.emit(self.group.group_id))
all_btn = QPushButton("全選/取消")
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
all_btn.clicked.connect(self._on_all_select_clicked)
self.all_btn_ref = all_btn # 保存按鈕參考（備用）
add_group_btn = QPushButton("+ 新增群組")
add_group_btn.setStyleSheet(BTN.format(bg='#4A9EFF', fg='white', hover='#3A8EEF'))
add_group_btn.clicked.connect(lambda: self.add_group_requested.emit())
grid_r1.addWidget(box_btn)
grid_r1.addWidget(all_btn)
grid_r1.addWidget(add_group_btn)
right.addLayout(grid_r1)
# 第二行 編輯分配 清除分組 刪除群組
grid_r2 = QHBoxLayout()
grid_r2.setSpacing(3)
assign_btn = QPushButton("編輯分配")
assign_btn.setStyleSheet(BTN.format(bg='#555', fg='#DDD', hover='#666'))
assign_btn.clicked.connect(
@@ -321,22 +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))
grid_r1.addWidget(assign_btn)
grid_r1.addWidget(clear_btn)
right.addLayout(grid_r1)
grid_r2 = QHBoxLayout()
grid_r2.setSpacing(3)
box_btn = QPushButton("框選")
box_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
box_btn.clicked.connect(
lambda: self.box_select_requested.emit(self.group.group_id))
all_btn = QPushButton("全選")
all_btn.setStyleSheet(BTN.format(bg='#64B5F6', fg='white', hover='#42A5F5'))
all_btn.clicked.connect(
lambda: self.select_all_requested.emit(self.group.group_id))
grid_r2.addWidget(box_btn)
grid_r2.addWidget(all_btn)
delete_group_btn = QPushButton("刪除群組")
delete_group_btn.setStyleSheet(BTN.format(bg='#EF5350', fg='white', hover='#E53935'))
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)
right.addLayout(grid_r2)
right.addStretch()
@@ -410,13 +424,15 @@ class GroupPanel(QWidget):
self.type_combo.currentTextChanged.connect(self._update_param_visibility)
# ── 組裝四欄：控制指令 > 任務規劃 > 任務參數 > 選取與分組 ──
cols.addLayout(left, 1)
# 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配
cols.addLayout(left, 0)
cols.addWidget(self._make_sep())
cols.addLayout(mid, 1)
cols.addLayout(mid, 0)
cols.addWidget(self._make_sep())
cols.addLayout(param_col, 1)
cols.addLayout(param_col, 0)
cols.addWidget(self._make_sep())
cols.addLayout(right, 1)
cols.addLayout(right, 0)
cols.addStretch() # 填充剩餘空間，使四列置左
layout.addLayout(cols)
@@ -451,6 +467,14 @@ class GroupPanel(QWidget):
self.status_label.setStyleSheet(
f"color: {self.group.color}; font-size: 11px; font-weight: bold;")
def _on_all_select_clicked(self):
"""全選按鈕點擊 - 發送信號給 gui.py 處理 toggle 邏輯"""
self.select_all_requested.emit(self.group.group_id)
def set_all_select_state(self, is_selected):
"""外部設置全選狀態（按鈕文本保持「全選/取消」）"""
self._is_all_selected = is_selected
def _update_param_visibility(self, _=None):
"""根據當前任務類型，顯示/隱藏對應的參數列"""
mission_type = self.type_combo.currentText()