|
|
|
|
@ -32,13 +32,17 @@ from mission_group import (
|
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
class ControlStationUI(QMainWindow):
|
|
|
|
|
VERSION = '1.0.1'
|
|
|
|
|
VERSION = '2.0.0'
|
|
|
|
|
|
|
|
|
|
def __init__(self):
|
|
|
|
|
super().__init__()
|
|
|
|
|
self.setWindowTitle(f'GCS v{self.VERSION}')
|
|
|
|
|
self.resize(1400, 900)
|
|
|
|
|
|
|
|
|
|
# 初始化消息隊列（用於線程安全的 GUI 更新）
|
|
|
|
|
import queue
|
|
|
|
|
self.message_queue = queue.Queue()
|
|
|
|
|
|
|
|
|
|
# 初始化ROS2
|
|
|
|
|
rclpy.init()
|
|
|
|
|
self.monitor = DroneMonitor()
|
|
|
|
|
@ -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("""
|
|
|
|
|
@ -205,6 +229,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()
|
|
|
|
|
|
|
|
|
|
@ -377,22 +404,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 +469,53 @@ 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)}")
|
|
|
|
|
import traceback
|
|
|
|
|
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 +533,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 +569,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 +634,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 +720,133 @@ 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
|
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
# 在後台線程中執行（避免阻塞 GUI）
|
|
|
|
|
def do_mode_changes_threaded():
|
|
|
|
|
print(f"\n 【後台線程】開始執行模式切換", flush=True)
|
|
|
|
|
import sys
|
|
|
|
|
import os
|
|
|
|
|
import time
|
|
|
|
|
|
|
|
|
|
# 确保 src 在 Python 路径中
|
|
|
|
|
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)
|
|
|
|
|
print(f" 【路徑】已添加: {src_path}", flush=True)
|
|
|
|
|
|
|
|
|
|
# 模式值映射
|
|
|
|
|
MODE_MAPPING = {
|
|
|
|
|
"STABILIZE": 0, "ACRO": 1, "ALT_HOLD": 2, "AUTO": 3,
|
|
|
|
|
"GUIDED": 4, "LOITER": 5, "RTL": 6, "CIRCLE": 7,
|
|
|
|
|
"LAND": 9, "DRIFT": 11, "SPORT": 13, "AUTOTUNE": 15,
|
|
|
|
|
"POSHOLD": 16, "BRAKE": 17, "SMART_RTL": 21,
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
custom_mode = MODE_MAPPING.get(mode)
|
|
|
|
|
if custom_mode is None:
|
|
|
|
|
msg = f"❌ 未知模式: {mode}"
|
|
|
|
|
print(f" {msg}", flush=True)
|
|
|
|
|
self.message_queue.put((msg, 2000))
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
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}"))
|
|
|
|
|
print(f"\n 【切換】{drone_id} → {mode} (mode={custom_mode})", flush=True)
|
|
|
|
|
try:
|
|
|
|
|
# 導入模式切換函數
|
|
|
|
|
from fc_network_apps.changeMode import change_mode
|
|
|
|
|
|
|
|
|
|
# 解析 sysid（從 drone_id 的最後一個數字）
|
|
|
|
|
sysid = int(drone_id.split('_')[-1])
|
|
|
|
|
print(f" └─ sysid={sysid}", flush=True)
|
|
|
|
|
|
|
|
|
|
# 調用 change_mode（同步操作）
|
|
|
|
|
try:
|
|
|
|
|
result = change_mode(
|
|
|
|
|
target_sysid=sysid,
|
|
|
|
|
custom_mode=float(custom_mode),
|
|
|
|
|
target_compid=0,
|
|
|
|
|
base_mode=1.0,
|
|
|
|
|
confirmation=0,
|
|
|
|
|
timeout_sec=2.0,
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
print(f" └─ 結果: success={result.success}", flush=True)
|
|
|
|
|
|
|
|
|
|
if result.success:
|
|
|
|
|
msg = f"✅ {drone_id} 切換成功"
|
|
|
|
|
print(f" {msg}", flush=True)
|
|
|
|
|
self.message_queue.put((msg, 2000))
|
|
|
|
|
else:
|
|
|
|
|
msg = f"❌ {drone_id} 切換失敗: {result.message}"
|
|
|
|
|
print(f" {msg}", flush=True)
|
|
|
|
|
self.message_queue.put((msg, 2000))
|
|
|
|
|
|
|
|
|
|
except Exception as service_error:
|
|
|
|
|
msg = f"❌ {drone_id} 服務調用錯誤: {str(service_error)}"
|
|
|
|
|
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)
|
|
|
|
|
import traceback
|
|
|
|
|
traceback.print_exc()
|
|
|
|
|
self.message_queue.put((msg, 2000))
|
|
|
|
|
self.message_queue.put((msg, 2000))
|
|
|
|
|
|
|
|
|
|
print(f"\n 【完成】模式切換任務結束\n", flush=True)
|
|
|
|
|
|
|
|
|
|
# 在後台線程執行
|
|
|
|
|
import threading
|
|
|
|
|
print(f" 【排隊】將任務提交至後台線程", flush=True)
|
|
|
|
|
thread = threading.Thread(target=do_mode_changes_threaded, daemon=True)
|
|
|
|
|
thread.start()
|
|
|
|
|
|
|
|
|
|
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 +882,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}
|
|
|
|
|
|
|
|
|
|
# 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 +951,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)
|
|
|
|
|
@ -751,12 +1056,47 @@ class ControlStationUI(QMainWindow):
|
|
|
|
|
else: self.monitor.selected_drones.discard(drone_id)
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
@ -1180,7 +1520,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,16 +1653,53 @@ 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)
|
|
|
|
|
|
|
|
|
|
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 定時器出現異常
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
def spin_ros(self):
|
|
|
|
|
try:
|
|
|
|
|
# 仅在 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)
|
|
|
|
|
except Exception as e:
|
|
|
|
|
print(f"ROS spin error: {e}")
|
|
|
|
|
print(f"ROS spin error: {e}", flush=True)
|
|
|
|
|
|
|
|
|
|
def closeEvent(self, event):
|
|
|
|
|
try:
|
|
|
|
|
for group in self.mission_groups.values():
|
|
|
|
|
if group.executor:
|
|
|
|
|
group.executor.stop()
|
|
|
|
|
@ -1334,7 +1710,16 @@ class ControlStationUI(QMainWindow):
|
|
|
|
|
receiver.stop()
|
|
|
|
|
self.monitor.destroy_node()
|
|
|
|
|
self.executor.shutdown()
|
|
|
|
|
except Exception as e:
|
|
|
|
|
print(f"⚠️ 清理資源時出錯: {e}", flush=True)
|
|
|
|
|
|
|
|
|
|
# 安全地 shutdown ROS2
|
|
|
|
|
try:
|
|
|
|
|
if rclpy.ok():
|
|
|
|
|
rclpy.shutdown()
|
|
|
|
|
except RuntimeError as e:
|
|
|
|
|
print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True)
|
|
|
|
|
|
|
|
|
|
event.accept()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
