diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 6e9ffcc..caa480c 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -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 + async def arm_drone(self, drone_id, arm): + """使用 CommandLongClient 執行 ARM/DISARM""" + try: + # 解析 drone_id 提取 sysid + parts = drone_id.split('_') + if len(parts) < 2: + print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}") + return False + sysid = int(parts[-1]) - client = self.takeoff_clients[drone_id] - if not client.wait_for_service(timeout_sec=1.0): - return False + action_name = "解鎖" if arm else "上鎖" + print(f"\n📢 [ARM] {drone_id} → {action_name}") - request = CommandTOL.Request() - request.altitude = altitude - request.min_pitch = 0.0 - request.yaw = 0.0 - - future = client.call_async(request) + # 在線程池中調用 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: + print(f"❌ [ARM] 例外錯誤: {e}") + traceback.print_exc() + return False + + async def takeoff_drone(self, drone_id, altitude): + """使用 CommandLongClient 執行無人機起飛""" try: - response = await future - return response.success + # 解析 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: - self.get_logger().error(f'Takeoff service call failed: {e}') + print(f"❌ [TAKEOFF] 例外錯誤: {e}") + traceback.print_exc() return False def send_setpoint(self, drone_id, x, y, z): diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1886305..4784545 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -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() \ No newline at end of file + main() \ No newline at end of file diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index a4043c2..60b4b8b 100644 --- a/src/GUI/mission_group.py +++ b/src/GUI/mission_group.py @@ -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()