@ -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.4 '
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（
self . monitor = DroneMonitor ( )
self . monitor . signals . update_signal . connect ( self . update_ui )
@ -48,16 +52,31 @@ 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 )
# 定时处理ROS事件
self . timer = QTimer ( )
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 更新（ ）
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 +183,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 : 5 px 12 px ; border - radius : 4 px ; font - size : 12 px ; 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 : 3 px 8 px ; border - radius : 3 px ; font - size : 12 px ; font - weight : bold ;
min - width : 30 px ; max - width : 30 px ; }
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 ( """
@ -205,6 +233,9 @@ class ControlStationUI(QMainWindow):
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 +384,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 +410,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 +475,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 +538,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 +574,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 +639,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 +725,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 完成（ \n " )
def _handle_group_takeoff ( self , group_id , altitude ) :
""" 群組內所有無人機起飛 """
@ -663,23 +840,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 +909,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,24 +1002,112 @@ 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 ]
if not group_drones : return
@ -890,7 +1241,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 +1312,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 +1391,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 +1528,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 +1661,135 @@ 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 ( )
# Clean up serial receivers
for receiver in self . monitor . serial_receivers :
receiver . stop ( )
# Clean up CommandLongClient
if self . monitor . command_long_client :
try :
self . monitor . command_long_client . destroy_node ( )
except :
pass
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 ( " ℹ ️ " , 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 ( " ℹ ️ " , flush = True )
print ( " ✓ [GUI 主程式] 應用程式完全退出 " , flush = True )
if __name__ == ' __main__ ' :
app = QApplication ( sys . argv )
station = ControlStationUI ( )
station . show ( )
app . exec ( )
main ( )
