diff --git a/src/GUI/command_sender.py b/src/GUI/command_sender.py index a0ddd12..0136bfe 100644 --- a/src/GUI/command_sender.py +++ b/src/GUI/command_sender.py @@ -16,6 +16,10 @@ from PyQt6.QtCore import QObject, pyqtSignal from pymavlink import mavutil +def _log(level, message): + print(f"[{level}] {message}") + + class CommandSender(ABC): """指令發送抽象介面""" @@ -63,7 +67,7 @@ class MavlinkSender(CommandSender): """ self.connection_string = connection_string self.mav = mavutil.mavlink_connection(connection_string) - print(f"MavlinkSender 已建立連線: {connection_string}") + _log("INFO", f"MavlinkSender 已建立連線: {connection_string}") def send_position_global(self, drone_id, sysid, lat, lon, alt): """發送 SET_POSITION_TARGET_GLOBAL_INT。drone_id 未使用,保留為介面相容。""" @@ -86,7 +90,7 @@ class MavlinkSender(CommandSender): if self.mav: self.mav.close() self.mav = None - print("MavlinkSender 已關閉") + _log("INFO", "MavlinkSender 已關閉") class Ros2CommandSender(QObject): @@ -162,4 +166,4 @@ class Ros2CommandSender(QObject): if not task.done(): task.cancel() self._pending.clear() - print("Ros2CommandSender 已關閉") \ No newline at end of file + _log("INFO", "Ros2CommandSender 已關閉") diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 3e83df5..c2298a8 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -20,6 +20,11 @@ from mavros_msgs.msg import State, VfrHud from nav_msgs.msg import Odometry from mavros_msgs.srv import CommandBool, CommandTOL + +def _log(level, message): + print(f"[{level}] {message}") + + # 確保 src 目錄在 Python 路徑中(用於 fc_network_apps 導入) _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) if _src_path not in sys.path: @@ -30,10 +35,9 @@ 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" 完整堆栈跟踪:") + _log("WARN", "無法導入 CommandLongClient") + _log("ERROR", f"錯誤: {e}") + _log("WARN", "這通常表示 ROS2 的 fc_interfaces 套件尚未編譯或安裝不完整") traceback.print_exc() CommandLongClient = None @@ -42,11 +46,18 @@ try: from fc_network_apps.navigation import PositionTargetGlobalIntClient except ImportError as e: import traceback - print(f"⚠️ 警告: 無法導入 PositionTargetGlobalIntClient") - print(f" 错误: {e}") + _log("WARN", "無法導入 PositionTargetGlobalIntClient") + _log("ERROR", f"錯誤: {e}") traceback.print_exc() PositionTargetGlobalIntClient = None +try: + from fc_interfaces.msg import AttitudeRaw +except ImportError as e: + _log("WARN", "無法導入 AttitudeRaw") + _log("ERROR", f"錯誤: {e}") + AttitudeRaw = None + class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -528,25 +539,25 @@ class DroneMonitor(Node): unique_name = f"cmd_long_client_{drone_id}_{self.client_counter}" client = CommandLongClient(node_name=unique_name) self.command_long_clients[drone_id] = client - print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (node={unique_name})") + _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (node={unique_name})") # 將新 client 添加到主執行器(這樣它的回調才能被處理) if self.executor: self.executor.add_node(client) - print(f" ✓ 將 {drone_id} 的 client 添加到主執行器") + _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") except TypeError: # 舊版 CommandLongClient 不支持 node_name 參數,使用預設 client = CommandLongClient() self.command_long_clients[drone_id] = client - print(f" ✓ 為 {drone_id} 創建新的 CommandLongClient (使用預設名稱)") + _log("INFO", f"已為 {drone_id} 建立 CommandLongClient (使用預設名稱)") if self.executor: self.executor.add_node(client) - print(f" ✓ 將 {drone_id} 的 client 添加到主執行器") + _log("INFO", f"已將 {drone_id} 的 CommandLongClient 加入主執行器") except Exception as e: - print(f"⚠️ 無法為 {drone_id} 創建 CommandLongClient: {e}") + _log("WARN", f"無法為 {drone_id} 建立 CommandLongClient: {e}") return None return self.command_long_clients[drone_id] @@ -561,12 +572,12 @@ class DroneMonitor(Node): unique_name = f"pos_target_client_{drone_id}_{self.pos_client_counter}" client = PositionTargetGlobalIntClient(node_name=unique_name) self.position_target_clients[drone_id] = client - print(f" ✓ 為 {drone_id} 創建新的 PositionTargetGlobalIntClient (node={unique_name})") + _log("INFO", f"已為 {drone_id} 建立 PositionTargetGlobalIntClient (node={unique_name})") if self.executor: self.executor.add_node(client) - print(f" ✓ 將 {drone_id} 的 position client 添加到主執行器") + _log("INFO", f"已將 {drone_id} 的 PositionTargetGlobalIntClient 加入主執行器") except Exception as e: - print(f"⚠️ 無法為 {drone_id} 創建 PositionTargetGlobalIntClient: {e}") + _log("WARN", f"無法為 {drone_id} 建立 PositionTargetGlobalIntClient: {e}") return None return self.position_target_clients[drone_id] @@ -593,7 +604,7 @@ class DroneMonitor(Node): if not hasattr(self, subs_attr): self.setup_drone(sys_id) else: - # 檢查既有訂閱是否包含 position_ned,如果不包含就添加(兼容舊訂閱) + # 檢查既有訂閱是否包含 position_ned / attitude,如果不包含就添加(兼容舊訂閱) subs = getattr(self, subs_attr, {}) if isinstance(subs, dict) and 'position_ned' not in subs: base_topic = f'/fc_network/vehicle/{sys_id}' @@ -608,6 +619,19 @@ class DroneMonitor(Node): setattr(self, subs_attr, subs) # 明確保存更新後的字典 except Exception as e: pass + if isinstance(subs, dict) and 'attitude' not in subs and AttitudeRaw is not None: + base_topic = f'/fc_network/vehicle/{sys_id}' + try: + attitude_sub = self.create_subscription( + AttitudeRaw, + f'{base_topic}/attitude', + lambda msg, sid=sys_id: self.attitude_callback(sid, msg), + 10 + ) + subs['attitude'] = attitude_sub + setattr(self, subs_attr, subs) + except Exception: + pass def setup_drone(self, sys_id): # sys_id 格式: sys11, sys12, ... @@ -662,6 +686,14 @@ class DroneMonitor(Node): 10 ) } + + if AttitudeRaw is not None: + subs['attitude'] = self.create_subscription( + AttitudeRaw, + f'{base_topic}/attitude', + lambda msg, sid=sys_id: self.attitude_callback(sid, msg), + 10 + ) setattr(self, f'drone_{sys_id}_subs', subs) @@ -699,22 +731,22 @@ class DroneMonitor(Node): # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: - print(f"❌ [SET_MODE] 無效的 drone_id 格式: {drone_id}") + _log("ERROR", 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}") + _log("ERROR", f"[SET_MODE] 未知模式: {mode_name}") return False - print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})") + _log("INFO", f"[SET_MODE] {drone_id} -> {mode_name} (custom_mode={custom_mode})") # 獲取或創建該 drone 專用的 client(避免多機並行時的競態條件) client = self.get_or_create_client(drone_id) if not client: - print(f"❌ [SET_MODE] CommandLongClient 無法初始化") + _log("ERROR", "[SET_MODE] CommandLongClient 無法初始化") return False # 直接調用 async 方法,無需線程池(避免嵌套執行器衝突) @@ -727,13 +759,13 @@ class DroneMonitor(Node): ) if result and result.success: - print(f"✅ [SET_MODE] 模式切換成功") + _log("INFO", f"[SET_MODE] {drone_id} 模式切換成功") return True else: - print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})") + _log("ERROR", f"[SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})") return False except Exception as e: - print(f"❌ [SET_MODE] 例外錯誤: {e}") + _log("ERROR", f"[SET_MODE] 例外錯誤: {e}") traceback.print_exc() return False @@ -743,17 +775,17 @@ class DroneMonitor(Node): # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: - print(f"❌ [ARM] 無效的 drone_id 格式: {drone_id}") + _log("ERROR", 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}") + _log("INFO", f"[ARM] {drone_id} -> {action_name}") # 獲取或創建該 drone 專用的 client(避免多機並行時的競態條件) client = self.get_or_create_client(drone_id) if not client: - print(f"❌ [ARM] CommandLongClient 無法初始化") + _log("ERROR", "[ARM] CommandLongClient 無法初始化") return False # 直接調用 async 方法,無需線程池(避免嵌套執行器衝突) @@ -765,13 +797,13 @@ class DroneMonitor(Node): ) if result and result.success: - print(f"✅ [ARM] {action_name}成功") + _log("INFO", f"[ARM] {drone_id} {action_name}成功") return True else: - print(f"❌ [ARM] {action_name}失敗 (message={result.message if result else 'None'})") + _log("ERROR", f"[ARM] {drone_id} {action_name}失敗 (message={result.message if result else 'None'})") return False except Exception as e: - print(f"❌ [ARM] 例外錯誤: {e}") + _log("ERROR", f"[ARM] 例外錯誤: {e}") traceback.print_exc() return False @@ -781,16 +813,16 @@ class DroneMonitor(Node): # 解析 drone_id 提取 sysid parts = drone_id.split('_') if len(parts) < 2: - print(f"❌ [TAKEOFF] 無效的 drone_id 格式: {drone_id}") + _log("ERROR", f"[TAKEOFF] 無效的 drone_id 格式: {drone_id}") return False sysid = int(parts[-1]) - print(f"\n📢 [TAKEOFF] {drone_id} → 起飛 (高度={altitude}m)") + _log("INFO", f"[TAKEOFF] {drone_id} -> 起飛 (高度={altitude}m)") # 獲取或創建該 drone 專用的 client(避免多機並行時的競態條件) client = self.get_or_create_client(drone_id) if not client: - print(f"❌ [TAKEOFF] CommandLongClient 無法初始化") + _log("ERROR", "[TAKEOFF] CommandLongClient 無法初始化") return False # 直接調用 async 方法,無需線程池(避免嵌套執行器衝突) @@ -802,13 +834,13 @@ class DroneMonitor(Node): ) if result and result.success: - print(f"✅ [TAKEOFF] 起飛成功") + _log("INFO", f"[TAKEOFF] {drone_id} 起飛成功") return True else: - print(f"❌ [TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})") + _log("ERROR", f"[TAKEOFF] 起飛失敗 (message={result.message if result else 'None'})") return False except Exception as e: - print(f"❌ [TAKEOFF] 例外錯誤: {e}") + _log("ERROR", f"[TAKEOFF] 例外錯誤: {e}") traceback.print_exc() return False @@ -840,17 +872,42 @@ class DroneMonitor(Node): return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) # callbacks - def attitude_callback(self, drone_id, msg): - if hasattr(msg, 'orientation'): - roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) - self.latest_data[(drone_id, 'attitude')] = { - 'roll': roll, - 'pitch': pitch, - 'yaw': yaw, - 'rates': (msg.angular_velocity.x, - msg.angular_velocity.y, - msg.angular_velocity.z) - } + def attitude_callback(self, sys_id, msg): + """處理姿態 topic,支援 AttitudeRaw 與 IMU 四元數格式。""" + actual_drone_id = self.sys_to_actual_id.get(sys_id, None) + if actual_drone_id is None: + return + + try: + if hasattr(msg, 'roll') and hasattr(msg, 'pitch') and hasattr(msg, 'yaw'): + data = { + 'roll': math.degrees(msg.roll), + 'pitch': math.degrees(msg.pitch), + 'yaw': math.degrees(msg.yaw), + 'rates': ( + getattr(msg, 'rollspeed', 0.0), + getattr(msg, 'pitchspeed', 0.0), + getattr(msg, 'yawspeed', 0.0), + ) + } + elif hasattr(msg, 'orientation'): + roll, pitch, yaw = self.quaternion_to_euler(msg.orientation) + data = { + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': ( + msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z, + ) + } + else: + return + + self.latest_data[(actual_drone_id, 'attitude')] = data + except Exception as e: + print(f"Error parsing attitude msg for {sys_id}: {e}") def battery_callback(self, sys_id, msg): # 使用映射獲取實際的 drone_id @@ -903,7 +960,7 @@ class DroneMonitor(Node): sys_num = sys_id.replace('sys', '') actual_drone_id = f's{assigned_socket_id}_{sys_num}' self.sys_to_actual_id[sys_id] = actual_drone_id - print(f"[DEBUG] summary_callback: 已創建映射 {sys_id} -> {actual_drone_id} (使用 sys_num)") + _log("INFO", f"summary_callback 已建立映射 {sys_id} -> {actual_drone_id} (使用 sys_num)") # 先發送連接類型資訊 self.signals.update_signal.emit('connection_type', actual_drone_id, { @@ -1003,6 +1060,9 @@ class DroneMonitor(Node): x = msg.pose.pose.position.y # NED 座標系中交換 x/y(與 local_pose 對齐) y = msg.pose.pose.position.x z = -msg.pose.pose.position.z # 將向下的 NED z 轉換為向上的高度(z 為負表示向下) + vx = msg.twist.twist.linear.y + vy = msg.twist.twist.linear.x + vz = -msg.twist.twist.linear.z # 儲存高度信息 self.latest_data[(actual_drone_id, 'altitude')] = { @@ -1015,6 +1075,13 @@ class DroneMonitor(Node): 'y': y, 'z': z } + + # 儲存速度資訊,供總覽頁「XY速度」欄位顯示 + self.latest_data[(actual_drone_id, 'velocity')] = { + 'vx': vx, + 'vy': vy, + 'vz': vz + } # 發送信號給 GUI 更新高度顯示 self.signals.update_signal.emit('altitude', actual_drone_id, { @@ -1039,5 +1106,5 @@ class DroneMonitor(Node): receiver = SerialMavlinkReceiver(port, baudrate, self.signals, connection_name, self) receiver.start() self.serial_receivers.append(receiver) - print(f"Started serial connection on {port} at {baudrate} baud") - return receiver \ No newline at end of file + _log("INFO", f"已啟動 Serial 連線: {port} @ {baudrate} baud") + return receiver diff --git a/src/GUI/gui.py b/src/GUI/gui.py index a332ed3..fc63e9d 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -4,8 +4,8 @@ from PyQt6.QtWidgets import (QApplication, QMainWindow, QVBoxLayout, QHBoxLayout QWidget, QLabel, QSplitter, QScrollArea, QSizePolicy, QTabWidget, QTableWidget, QTableWidgetItem, QHeaderView, QPushButton, QCheckBox, QLineEdit, - QComboBox, QDialog) -from PyQt6.QtCore import Qt, QTimer + QComboBox, QDialog, QPlainTextEdit) +from PyQt6.QtCore import Qt, QTimer, QObject, pyqtSignal from PyQt6.QtGui import QColor import sys import asyncio @@ -14,6 +14,11 @@ import subprocess import time import traceback + +def _log(level, message): + print(f"[{level}] {message}", flush=True) + + # 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver from map_layout import DroneMap @@ -32,8 +37,41 @@ from mission_group import ( ) # ================================================================================ + +class StreamRedirector(QObject): + """將 stdout/stderr 同步轉發到 Qt UI。""" + text_written = pyqtSignal(str) + + def __init__(self, original_stream=None, parent=None): + super().__init__(parent) + self.original_stream = original_stream + self._buffer = "" + + def write(self, text): + if self.original_stream: + self.original_stream.write(text) + + if not text: + return + + self._buffer += text + while "\n" in self._buffer: + line, self._buffer = self._buffer.split("\n", 1) + line = line.strip() + if line: + self.text_written.emit(line) + + def flush(self): + if self.original_stream: + self.original_stream.flush() + + line = self._buffer.strip() + if line: + self.text_written.emit(line) + self._buffer = "" + class ControlStationUI(QMainWindow): - VERSION = '2.0.8' + VERSION = '2.1.0' def __init__(self): super().__init__() @@ -78,6 +116,8 @@ class ControlStationUI(QMainWindow): # 快取消息數據,以便在沒有新消息時使用上一次的值 self._message_cache = {} + self.message_history = [] + self.max_message_history = 500 # 初始化UI self.drones = {} @@ -137,6 +177,7 @@ class ControlStationUI(QMainWindow): # ================================================================================ self.init_ui() + self._setup_stream_redirector() def init_ui(self): main_splitter = QSplitter(Qt.Orientation.Horizontal) @@ -160,7 +201,11 @@ class ControlStationUI(QMainWindow): self.overview_table = OverviewTable() self.left_tab.addTab(self.overview_table, "總覽") - # — 分頁 3:通訊設定 + # — 分頁 3:訊息歷史 + self.message_history_tab = self._create_message_history_tab() + self.left_tab.addTab(self.message_history_tab, "紀錄") + + # — 分頁 4:通訊設定 self.comm_panel = CommPanel() self.comm_panel.udp_connection_added.connect(self.handle_udp_connection_added) self.comm_panel.ws_connection_added.connect(self.handle_ws_connection_added) @@ -259,6 +304,95 @@ class ControlStationUI(QMainWindow): main_splitter.setSizes([400, 1000]) self.setCentralWidget(main_splitter) + self.statusBar().messageChanged.connect(self._on_status_bar_message_changed) + + def _create_message_history_tab(self): + """建立左側訊息歷史分頁。""" + widget = QWidget() + layout = QVBoxLayout(widget) + layout.setContentsMargins(10, 10, 10, 10) + layout.setSpacing(8) + + header_layout = QHBoxLayout() + title = QLabel("操作與訊息歷史") + title.setStyleSheet("color: #DDD; font-size: 14px; font-weight: bold;") + header_layout.addWidget(title) + header_layout.addStretch() + + clear_btn = QPushButton("清空") + clear_btn.setStyleSheet(""" + QPushButton { background-color: #555; color: white; border: none; + padding: 5px 10px; border-radius: 4px; font-size: 12px; } + QPushButton:hover { background-color: #666; } + """) + clear_btn.clicked.connect(self._clear_message_history) + header_layout.addWidget(clear_btn) + layout.addLayout(header_layout) + + self.message_history_view = QPlainTextEdit() + self.message_history_view.setReadOnly(True) + self.message_history_view.setStyleSheet(""" + QPlainTextEdit { + background-color: #1E1E1E; + color: #DDD; + border: 1px solid #444; + border-radius: 6px; + padding: 6px; + font-family: monospace; + font-size: 12px; + } + """) + self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...") + layout.addWidget(self.message_history_view) + + return widget + + def _clear_message_history(self): + """清空訊息歷史。""" + self.message_history.clear() + if hasattr(self, 'message_history_view'): + self.message_history_view.clear() + + def _on_status_bar_message_changed(self, message): + """同步狀態列訊息到歷史紀錄。""" + if not message: + return + timestamp = time.strftime("%H:%M:%S") + entry = f"[{timestamp}] {message}" + self.message_history.append(entry) + if len(self.message_history) > self.max_message_history: + self.message_history = self.message_history[-self.max_message_history:] + + if hasattr(self, 'message_history_view'): + self.message_history_view.setPlainText("\n".join(self.message_history)) + scrollbar = self.message_history_view.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) + + def _setup_stream_redirector(self): + """將 stdout/stderr 同步到左下角狀態列與訊息紀錄。""" + self._original_stdout = sys.stdout + self._original_stderr = sys.stderr + + self.stdout_redirector = StreamRedirector(self._original_stdout, self) + self.stderr_redirector = StreamRedirector(self._original_stderr, self) + self.stdout_redirector.text_written.connect(self.show_in_bottom_left) + self.stderr_redirector.text_written.connect(self.show_in_bottom_left) + + sys.stdout = self.stdout_redirector + sys.stderr = self.stderr_redirector + + def _restore_stream_redirector(self): + """還原 stdout/stderr,避免關閉視窗後仍寫入 UI。""" + if getattr(self, '_original_stdout', None) is not None: + sys.stdout = self._original_stdout + if getattr(self, '_original_stderr', None) is not None: + sys.stderr = self._original_stderr + + def show_in_bottom_left(self, text): + """將重導向的輸出顯示在左下角狀態列。""" + if not text: + return + self.statusBar().showMessage(text, 5000) # ================================================================================ @@ -384,7 +518,7 @@ class ControlStationUI(QMainWindow): status_label.setToolTip("運行中") self.statusBar().showMessage(f"已啟動 Serial 連接: {conn['port']}", 3000) except Exception as e: - print(f"❌ Serial 連接啟動失敗: {str(e)}") + _log("ERROR", f"Serial 連接啟動失敗: {str(e)}") traceback.print_exc() self.statusBar().showMessage(f"啟動 Serial 連接失敗: {str(e)}", 5000) @@ -410,39 +544,25 @@ 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}") - + _log("INFO", f"切換模式請求: drone={drone_id}, mode={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) - 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") + _log("INFO", f"{action_text} 請求已送出") + loop.create_task(self.handle_service_response(coro, action_text)) def handle_takeoff(self, drone_id): loop = asyncio.get_event_loop() @@ -453,7 +573,7 @@ class ControlStationUI(QMainWindow): """發送位置命令到 active group 的所有選中無人機""" group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return try: @@ -481,34 +601,18 @@ 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})") + _log("INFO", f"{action} 成功") self.statusBar().showMessage(f"{action} 成功", 3000) else: - print(f"\n❌ {action} 失敗 (result={result})") + _log("WARN", f"{action} 失敗") self.statusBar().showMessage(f"{action} 失敗", 3000) except asyncio.CancelledError: - print(f"⚠️ {action} 被取消") + _log("WARN", f"{action} 已取消") except Exception as e: - print(f"❌ {action} 錯誤: {str(e)}") + _log("ERROR", f"{action} 錯誤: {str(e)}") traceback.print_exc() self.statusBar().showMessage(f"{action} 錯誤: {str(e)}", 3000) @@ -516,38 +620,33 @@ class ControlStationUI(QMainWindow): """解鎖 active group 的所有選中無人機""" group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return selected = list(group.selected_drone_ids) if not selected: - self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000) + self.statusBar().showMessage("尚未選擇任何無人機", 3000) return - - print(f"\n📢 [GUI] handle_arm_selected 被調用") - print(f" 已選擇的無人機: {selected}") + _log("INFO", f"批次解鎖請求: {', '.join(selected)}") loop = asyncio.get_event_loop() 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): """起飛 active group 的所有選中無人機""" group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return selected = list(group.selected_drone_ids) if not selected: - self.statusBar().showMessage("⚠ 尚未選擇任何無人機", 3000) + self.statusBar().showMessage("尚未選擇任何無人機", 3000) return loop = asyncio.get_event_loop() @@ -774,7 +873,7 @@ class ControlStationUI(QMainWindow): if blocked: self.statusBar().showMessage( - f"⚠ Group {group_id}: {len(blocked)} 台已被其他 group 勾選,未加入", 4000 + f"Group {group_id}: {len(blocked)} 台已被其他 group 勾選,未加入", 4000 ) else: self.statusBar().showMessage( @@ -811,7 +910,7 @@ class ControlStationUI(QMainWindow): if not group: return if group.planned_waypoints is None: - self.statusBar().showMessage(f"⚠ Group {group_id}: 請先規劃任務", 3000) + self.statusBar().showMessage(f"Group {group_id}: 請先規劃任務", 3000) return if group.executor is None: self._create_executor_for_group(group) @@ -819,7 +918,7 @@ class ControlStationUI(QMainWindow): panel = self.group_panels.get(group_id) if panel: panel.update_status() - self.statusBar().showMessage(f"🚀 Group {group_id}: 任務已啟動", 3000) + self.statusBar().showMessage(f"Group {group_id}: 任務已啟動", 3000) def _handle_group_pause(self, group_id): """暫停/恢復群組任務""" @@ -828,10 +927,10 @@ class ControlStationUI(QMainWindow): return if group.executor.state == MissionState.RUNNING: group.executor.pause() - self.statusBar().showMessage(f"⏸ Group {group_id}: 任務已暫停", 3000) + self.statusBar().showMessage(f"Group {group_id}: 任務已暫停", 3000) elif group.executor.state == MissionState.PAUSED: group.executor.resume() - self.statusBar().showMessage(f"▶ Group {group_id}: 任務已恢復", 3000) + self.statusBar().showMessage(f"Group {group_id}: 任務已恢復", 3000) panel = self.group_panels.get(group_id) if panel: panel.update_status() @@ -849,54 +948,44 @@ class ControlStationUI(QMainWindow): if panel: panel.update_status() panel.clear_mission_info() - self.statusBar().showMessage(f"■ Group {group_id}: 任務已停止", 3000) + self.statusBar().showMessage(f"Group {group_id}: 任務已停止", 3000) 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) + _log("ERROR", f"找不到群組: {group_id}") return if not group.selected_drone_ids: - print(f"⚠️ 群組中沒有無人機", flush=True) + _log("WARN", f"Group {group_id} 中沒有無人機") self.statusBar().showMessage(f"群組 {group_id} 中沒有無人機", 3000) return - - print(f" 準備為 {len(group.selected_drone_ids)} 台無人機切換模式...", flush=True) - self.statusBar().showMessage(f"正在切換模式...", 1000) + _log("INFO", f"Group {group_id} 準備切換模式為 {mode}") + self.statusBar().showMessage("正在切換模式", 1000) # 使用 asyncio 執行(通過事件循環) async def do_mode_changes_async(): - print(f"\n 【異步任務】開始執行模式切換", flush=True) - for drone_id in group.selected_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) + msg = f"{drone_id} 切換成功" + _log("INFO", msg) self.message_queue.put((msg, 2000)) else: - msg = f"❌ {drone_id} 切換失敗" - print(f" {msg}", flush=True) + msg = f"{drone_id} 切換失敗" + _log("WARN", msg) self.message_queue.put((msg, 2000)) except Exception as e: - msg = f"❌ {drone_id} 錯誤: {str(e)}" - print(f" {msg}", flush=True) + msg = f"{drone_id} 錯誤: {str(e)}" + _log("ERROR", msg) traceback.print_exc() self.message_queue.put((msg, 2000)) - - print(f"\n 【完成】模式切換任務結束\n", flush=True) # 通過事件循環提交異步任務 - print(f" 【排隊】將任務提交至事件循環", flush=True) loop = asyncio.get_event_loop() asyncio.run_coroutine_threadsafe( do_mode_changes_async(), @@ -905,34 +994,22 @@ class ControlStationUI(QMainWindow): 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") + _log("WARN", f"找不到群組: {group_id}") return - print(f" 群組內無人機: {group.selected_drone_ids}") loop = asyncio.get_event_loop() - print(f" 事件循環: {loop}") + _log("INFO", f"Group {group_id} 批次解鎖 {len(group.selected_drone_ids)} 台無人機") for drone_id in group.selected_drone_ids: - 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( + 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): """群組內所有無人機起飛""" @@ -983,7 +1060,7 @@ class ControlStationUI(QMainWindow): if blocked: self.statusBar().showMessage( - f"⚠ Group {group_id}: 部分 drone 已被其他 group 勾選,僅加入 {len(allowed)} 台", 4000 + f"Group {group_id}: 部分 drone 已被其他 group 勾選,僅加入 {len(allowed)} 台", 4000 ) else: self.statusBar().showMessage( @@ -1049,7 +1126,7 @@ class ControlStationUI(QMainWindow): """刪除一個任務群組""" # 檢查是否只有一個群組,如果是就禁止刪除 if len(self.mission_groups) <= 1: - self.statusBar().showMessage("⚠️ 至少需要保留一個群組!", 3000) + self.statusBar().showMessage("至少需要保留一個群組", 3000) return if group_id not in self.mission_groups: @@ -1100,7 +1177,7 @@ class ControlStationUI(QMainWindow): panel = self.group_panels.get(group_id) if panel: panel.update_status() - self.statusBar().showMessage(f"✅ Group {group_id}: 所有無人機已完成任務", 5000) + self.statusBar().showMessage(f"Group {group_id}: 所有無人機已完成任務", 5000) # ================================================================================ # UI 更新 @@ -1152,7 +1229,7 @@ class ControlStationUI(QMainWindow): if not selectable: if blocked: self.statusBar().showMessage( - f"⚠ Socket {socket_id} 的 drone 已被其他 group 勾選,無法操作", 4000 + f"Socket {socket_id} 的 drone 已被其他 group 勾選,無法操作", 4000 ) self.refresh_selection_ui() return @@ -1165,7 +1242,7 @@ class ControlStationUI(QMainWindow): group.selected_drone_ids.update(selectable) if blocked: self.statusBar().showMessage( - f"⚠ 以下 drone 已被其他 group 勾選,略過: {', '.join(blocked)}", 4000 + f"以下 drone 已被其他 group 勾選,略過: {', '.join(blocked)}", 4000 ) self.refresh_selection_ui() @@ -1217,7 +1294,7 @@ class ControlStationUI(QMainWindow): if is_checked: if self._is_drone_selected_by_other_group(drone_id, group.group_id): self.statusBar().showMessage( - f"⚠ {drone_id} 已被其他 group 勾選,無法加入 Group {group.group_id}", 3000 + f"{drone_id} 已被其他 group 勾選,無法加入 Group {group.group_id}", 3000 ) self.refresh_selection_ui() return @@ -1239,7 +1316,7 @@ class ControlStationUI(QMainWindow): # 嘗試勾選前先檢查是否被其他 group 使用 if self._is_drone_selected_by_other_group(drone_id, group.group_id): self.statusBar().showMessage( - f"⚠ {drone_id} 已被其他 group 勾選,無法加入 Group {group.group_id}", 3000 + f"{drone_id} 已被其他 group 勾選,無法加入 Group {group.group_id}", 3000 ) self.refresh_selection_ui() return @@ -1285,7 +1362,7 @@ class ControlStationUI(QMainWindow): """處理地圖點擊事件 — 根據 active group 的任務類型規劃""" group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return mode_map = { @@ -1298,16 +1375,16 @@ class ControlStationUI(QMainWindow): selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) + self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000) return - print(f"地圖點擊: {lat:.6f}, {lon:.6f} → Group {group.group_id} ({group.mission_type})") + _log("INFO", f"地圖點擊: {lat:.6f}, {lon:.6f} -> Group {group.group_id} ({group.mission_type})") panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} base_alt = params.get('base_altitude', params.get('altitude', 10.0)) target_gps = (lat, lon, base_alt) self.statusBar().showMessage( - f"⏳ Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台) ...", 2000) + f"Group {group.group_id}: 正在規劃 {group.mission_type} ({len(selected_drones)} 台)", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) @@ -1344,10 +1421,10 @@ class ControlStationUI(QMainWindow): total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ Group {group.group_id}: {group.mission_type} 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 + f"Group {group.group_id}: {group.mission_type} 規劃完成,{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ Group {group.group_id}: 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"Group {group.group_id}: 規劃失敗: {str(e)}", 5000) traceback.print_exc() # ================================================================================ @@ -1357,28 +1434,28 @@ class ControlStationUI(QMainWindow): def handle_rectangle_selected(self, points_json): group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return if group.mission_type != 'GRID_SWEEP': return # 非 Grid Sweep 模式不處理矩形選取 selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) + self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000) return - print(f"矩形選取 → Group {group.group_id}: {points_json}") + _log("INFO", f"矩形選取 -> Group {group.group_id}: {points_json}") try: rect_corners = [(p[0], p[1]) for p in json.loads(points_json)] except (json.JSONDecodeError, IndexError): - self.statusBar().showMessage("❌ 矩形座標解析失敗", 3000) + self.statusBar().showMessage("矩形座標解析失敗", 3000) return panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} base_alt = params.get('altitude', 10.0) self.statusBar().showMessage( - f"⏳ Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台) ...", 2000) + f"Group {group.group_id}: 正在規劃 Grid Sweep ({len(selected_drones)} 台)", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) if drone_gps_positions is None: @@ -1419,10 +1496,10 @@ class ControlStationUI(QMainWindow): total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ Group {group.group_id}: Grid Sweep 規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 + f"Group {group.group_id}: Grid Sweep 規劃完成,{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"Group {group.group_id}: Grid Sweep 規劃失敗: {str(e)}", 5000) traceback.print_exc() # ================================================================================ @@ -1432,14 +1509,14 @@ class ControlStationUI(QMainWindow): def handle_route_confirmed(self, points_json): group = self._get_active_group() if not group: - self.statusBar().showMessage("⚠ 請先建立任務群組", 3000) + self.statusBar().showMessage("請先建立任務群組", 3000) return if group.mission_type != 'LEADER_FOLLOWER': return selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: - self.statusBar().showMessage(f"⚠ Group {group.group_id}: 請先分配無人機", 3000) + self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000) return # LEADER_FOLLOWER:把指定的領隊放到 rank 0 @@ -1447,23 +1524,23 @@ class ControlStationUI(QMainWindow): if leader and leader in selected_drones: selected_drones = [leader] + [d for d in selected_drones if d != leader] - print(f"路徑確認 → Group {group.group_id}: {points_json}") + _log("INFO", f"路徑確認 -> Group {group.group_id}: {points_json}") try: route_points = json.loads(points_json) route_waypoints = [(p[0], p[1]) for p in route_points] except (json.JSONDecodeError, IndexError): - self.statusBar().showMessage("❌ 路徑座標解析失敗", 3000) + self.statusBar().showMessage("路徑座標解析失敗", 3000) return if len(route_waypoints) < 2: - self.statusBar().showMessage("⚠ 至少需要 2 個路徑點", 3000) + self.statusBar().showMessage("至少需要 2 個路徑點", 3000) return panel = self.group_panels.get(group.group_id) params = panel.get_mission_params() if panel else {} base_alt = params.get('altitude', 10.0) self.statusBar().showMessage( - f"⏳ Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台) ...", 2000) + f"Group {group.group_id}: 正在規劃跟隨模式 ({len(selected_drones)} 台)", 2000) try: drone_gps_positions = self._collect_drone_gps(selected_drones, base_alt) @@ -1507,10 +1584,10 @@ class ControlStationUI(QMainWindow): total_wps = sum(len(wps) for wps in waypoints_per_drone) self.statusBar().showMessage( - f"✓ Group {group.group_id}: 跟隨模式規劃完成!{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 + f"Group {group.group_id}: 跟隨模式規劃完成,{len(selected_drones)} 台,共 {total_wps} 個航點", 5000 ) except Exception as e: - self.statusBar().showMessage(f"❌ Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000) + self.statusBar().showMessage(f"Group {group.group_id}: 跟隨模式規劃失敗: {str(e)}", 5000) traceback.print_exc() # ================================================================================ @@ -1519,20 +1596,20 @@ class ControlStationUI(QMainWindow): def on_drone_waypoint_reached(self, drone_id, wp_index, total): if wp_index >= total: - self.statusBar().showMessage(f"📍 {drone_id} 完成所有航點", 3000) + self.statusBar().showMessage(f"{drone_id} 已完成所有航點", 3000) else: - self.statusBar().showMessage(f"📍 {drone_id} → WP {wp_index}/{total}", 2000) + self.statusBar().showMessage(f"{drone_id} 到達 WP {wp_index}/{total}", 2000) def _on_task_status_changed(self, drone_id, status, message): """MissionExecutor.task_status_changed slot:把 goto 失敗/重試/fallback/barrier 丟到 status bar""" if status == "retrying": - self.statusBar().showMessage(f"⚠ {drone_id} {message}", 4000) + self.statusBar().showMessage(f"{drone_id} {message}", 4000) elif status == "fallback_loiter": - self.statusBar().showMessage(f"❌ {drone_id} {message}", 8000) + self.statusBar().showMessage(f"{drone_id} {message}", 8000) elif status == "waiting_at_barrier": - self.statusBar().showMessage(f"⏸ {drone_id} {message}", 3000) + self.statusBar().showMessage(f"{drone_id} {message}", 3000) elif status == "normal": - self.statusBar().showMessage(f"▶ {drone_id} {message or '已恢復'}", 2000) + self.statusBar().showMessage(f"{drone_id} {message or '已恢復'}", 2000) # ================================================================================ # 輔助方法 @@ -1551,7 +1628,7 @@ class ControlStationUI(QMainWindow): alt_drone = pos[2] if len(pos) > 2 else base_alt drone_gps_positions.append((lat_drone, lon_drone, alt_drone)) else: - self.statusBar().showMessage(f"⚠ 找不到 {drone_id} 的位置資料", 3000) + self.statusBar().showMessage(f"找不到 {drone_id} 的位置資料", 3000) return None return drone_gps_positions @@ -1580,7 +1657,7 @@ class ControlStationUI(QMainWindow): script_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'validation', 'verify_waypoints.py') subprocess.Popen([sys.executable, script_path, '--file', json_path]) - print(f"驗證視窗已啟動: {json_path}") + _log("INFO", f"驗證視窗已啟動: {json_path}") def show_planned_waypoints(self, group=None): pw = group.planned_waypoints if group else None @@ -1739,7 +1816,7 @@ class ControlStationUI(QMainWindow): panel._last_roll = roll panel._last_pitch = pitch if hasattr(panel, 'update_attitude'): - heading = self.drone_headings.get(drone_id, 0) + heading = self.drone_headings.get(drone_id, yaw) panel.update_attitude(heading, roll, pitch) elif msg_type == 'gps': @@ -1780,7 +1857,7 @@ class ControlStationUI(QMainWindow): elapsed = (time.time() - start_time) * 1000 if elapsed > 33: - print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)") + _log("WARN", f"UI 更新耗時 {elapsed:.1f}ms (目標 <33ms)") finally: # ✅ 步驟 3: 恢復表格重繪(所有資料已填好,一次性重繪) @@ -1800,7 +1877,7 @@ class ControlStationUI(QMainWindow): except: break except Exception as e: - print(f"❌ 消息隊列處理錯誤: {e}", flush=True) + _log("ERROR", f"訊息佇列處理錯誤: {e}") traceback.print_exc() def _spin_asyncio(self): @@ -1831,15 +1908,16 @@ class ControlStationUI(QMainWindow): except RuntimeError as e: # ROS2 context 被破坏或不可用 if "Context" in str(e) or "context" in str(e).lower(): - print(f"⚠️ ROS2 context 錯誤(忽略): {e}", flush=True) + _log("WARN", f"ROS2 context 錯誤(已忽略): {e}") else: - print(f"ROS spin error: {e}", flush=True) + _log("ERROR", f"ROS spin error: {e}") traceback.print_exc() except Exception as e: - print(f"ROS spin error: {e}", flush=True) + _log("ERROR", f"ROS spin error: {e}") traceback.print_exc() def closeEvent(self, event): + self._restore_stream_redirector() try: for group in self.mission_groups.values(): if group.executor: @@ -1867,7 +1945,7 @@ class ControlStationUI(QMainWindow): self.monitor.destroy_node() self.executor.shutdown() except Exception as e: - print(f"⚠️ 清理資源時出錯: {e}", flush=True) + _log("WARN", f"清理資源時發生錯誤: {e}") traceback.print_exc() # 安全地 shutdown ROS2 @@ -1875,7 +1953,7 @@ class ControlStationUI(QMainWindow): if rclpy.ok(): rclpy.shutdown() except RuntimeError as e: - print(f"⚠️ ROS2 shutdown 錯誤: {e}", flush=True) + _log("WARN", f"ROS2 shutdown 錯誤: {e}") traceback.print_exc() event.accept() @@ -1894,36 +1972,36 @@ def main(): """ # 第一步:在最外層只初始化一次 ROS2(終極防護) # 添加 rclpy.ok() 檢查,防止重複初始化導致 "Context.init() must only be called once" 錯誤 - print("🚀 [GUI 主程式] 檢查 ROS2 初始化狀態...", flush=True) + _log("INFO", "GUI 主程式檢查 ROS2 初始化狀態") if not rclpy.ok(): - print("🚀 [GUI 主程式] ROS2 未初始化,開始初始化...", flush=True) + _log("INFO", "ROS2 尚未初始化,開始初始化") rclpy.init() - print("✅ [GUI 主程式] ROS2 已全局初始化(由 GUI 主程式霸佔)", flush=True) + _log("INFO", "ROS2 已完成全域初始化") else: - print("ℹ️ [GUI 主程式] ROS2 已初始化,跳過重複初始化", flush=True) + _log("INFO", "ROS2 已初始化,略過重複初始化") try: # 第二步:啟動 Qt 應用程式 - print("🚀 [GUI 主程式] 啟動 Qt 應用程式...", flush=True) + _log("INFO", "啟動 Qt 應用程式") app = QApplication(sys.argv) station = ControlStationUI() station.show() - print("✓ [GUI 主程式] 應用程式已啟動", flush=True) + _log("INFO", "應用程式已啟動") # 第三步:進入 Qt 事件循環(阻塞直到用戶關閉應用) - print("🎯 [GUI 主程式] 進入主事件循環,等待用戶操作...", flush=True) + _log("INFO", "進入主事件循環") sys.exit(app.exec()) finally: # 第四步:只有當 GUI 視窗被關閉時,才做 ROS2 cleanup(終極防護) # 這裡確保 ROS2 被正確且安全地關閉 - print("\n🛑 [GUI 主程式] 應用程式關閉,正在清理 ROS2 資源...", flush=True) + _log("INFO", "應用程式關閉,開始清理 ROS2 資源") if rclpy.ok(): rclpy.shutdown() - print("✓ [GUI 主程式] ROS2 已安全關閉", flush=True) + _log("INFO", "ROS2 已安全關閉") else: - print("ℹ️ [GUI 主程式] ROS2 已關閉或不可用,無需重複 shutdown", flush=True) - print("✓ [GUI 主程式] 應用程式完全退出", flush=True) + _log("INFO", "ROS2 已關閉或不可用,無需重複 shutdown") + _log("INFO", "應用程式已完全退出") if __name__ == '__main__': diff --git a/src/GUI/map_layout.py b/src/GUI/map_layout.py index 2be9cb9..4f36017 100644 --- a/src/GUI/map_layout.py +++ b/src/GUI/map_layout.py @@ -3,6 +3,11 @@ from PyQt6.QtWebEngineWidgets import QWebEngineView from PyQt6.QtCore import QTimer, pyqtSignal, QObject, pyqtSlot from PyQt6.QtWebChannel import QWebChannel + +def _log(level, message): + print(f"[{level}] {message}") + + class DroneMap: """無人機地圖類別 - 負責管理 Leaflet 地圖顯示""" @@ -874,7 +879,7 @@ class DroneMap: if ok: self.map_loaded = True else: - print("⚠️ 地圖加載失敗") + _log("ERROR", "地圖載入失敗") def update_drone_position(self, drone_id, lat, lon, heading): """更新無人機位置(加入待處理隊列)""" @@ -924,20 +929,24 @@ class DroneMap: f"{target_lat:.6f}, {target_lon:.6f});" ) self.map_view.page().runJavaScript(js_code) - print(f"📍 地圖已繪製 Group {group_id} 任務規劃: " - f"C({center_lat:.6f}, {center_lon:.6f}) -> T({target_lat:.6f}, {target_lon:.6f})") + _log( + "INFO", + f"地圖已繪製 Group {group_id} 任務規劃: " + f"C({center_lat:.6f}, {center_lon:.6f}) -> " + f"T({target_lat:.6f}, {target_lon:.6f})", + ) def clear_mission_plan(self): """清除地圖上所有任務規劃標記""" if self.map_loaded: self.map_view.page().runJavaScript("clearAllMissionPlans();") - print("🗑️ 地圖已清除所有任務規劃") + _log("INFO", "地圖已清除所有任務規劃") def clear_mission_plan_for_group(self, group_id): """清除指定群組的任務規劃標記""" if self.map_loaded: self.map_view.page().runJavaScript(f"clearMissionPlanForGroup('{group_id}');") - print(f"🗑️ 地圖已清除 Group {group_id} 任務規劃") + _log("INFO", f"地圖已清除 Group {group_id} 任務規劃") def set_mission_mode(self, mode): """從 Python 端切換地圖的任務模式(觸發框選/路徑標記等)""" @@ -1033,52 +1042,56 @@ class MapBridge(QObject): def clearAllDroneSelection(self): """供 JavaScript 調用的方法 - 清除所有無人機選擇""" self.clear_all_drone_selection.emit() - print("🗑️ 清除所有無人機選擇") + _log("INFO", "已清除所有無人機選擇") @pyqtSlot() def toggleSelectAllDrones(self): """供 JavaScript 調用的方法 - 切換全選/取消全選所有無人機""" self.select_all_drones.emit() - print("🔄 切換全選無人機") + _log("INFO", "已切換全選無人機") @pyqtSlot(float, float, float, float) def startMissionSignal(self, center_lat, center_lon, target_lat, target_lon): """供 JavaScript 調用的方法 - 開始任務""" self.start_mission_signal.emit(center_lat, center_lon, target_lat, target_lon) - print(f"🚀 開始任務信號已發出: C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})") + _log( + "INFO", + f"已發出開始任務信號: " + f"C({center_lat}, {center_lon}) -> T({target_lat}, {target_lon})", + ) @pyqtSlot() def pauseMissionSignal(self): """供 JavaScript 調用的方法 - 暫停任務""" self.pause_mission_signal.emit() - print("⏸️ 暫停任務信號已發出") + _log("INFO", "已發出暫停任務信號") @pyqtSlot(str) def rectangleSelected(self, points_json): """供 JavaScript 調用的方法 - 矩形選擇完成""" self.rectangle_selected.emit(points_json) - print(f"📦 矩形區域已選擇: {points_json}") + _log("INFO", f"矩形區域已選擇: {points_json}") @pyqtSlot(str) def polygonSelected(self, points_json): """供 JavaScript 調用的方法 - 多邊形選擇完成""" self.polygon_selected.emit(points_json) - print(f"🔷 多邊形區域已選擇: {points_json}") + _log("INFO", f"多邊形區域已選擇: {points_json}") @pyqtSlot(str) def missionModeChanged(self, mode): """供 JavaScript 調用的方法 - 任務模式切換""" self.mission_mode_changed.emit(mode) - print(f"🔄 任務模式切換: {mode}") + _log("INFO", f"任務模式已切換: {mode}") @pyqtSlot(str) def routeConfirmed(self, points_json): """供 JavaScript 調用的方法 - 路徑確認""" self.route_confirmed.emit(points_json) - print(f"📍 路徑已確認: {points_json}") + _log("INFO", f"路徑已確認: {points_json}") @pyqtSlot(str) def droneBoxSelected(self, drone_ids_json): """供 JavaScript 調用的方法 - 框選無人機完成""" self.drone_box_selected.emit(drone_ids_json) - print(f"📦 框選無人機: {drone_ids_json}") \ No newline at end of file + _log("INFO", f"框選無人機完成: {drone_ids_json}") diff --git a/src/GUI/mission_executor.py b/src/GUI/mission_executor.py index e5bd11f..e086362 100644 --- a/src/GUI/mission_executor.py +++ b/src/GUI/mission_executor.py @@ -19,6 +19,10 @@ from enum import Enum from PyQt6.QtCore import QObject, QTimer, pyqtSignal +def _log(level, message): + print(f"[{level}] {message}") + + class MissionState(Enum): """整體任務狀態""" IDLE = "idle" @@ -109,7 +113,7 @@ class MissionExecutor(QObject): def start(self, planned_waypoints): if self.state == MissionState.RUNNING: - print("任務已在執行中") + _log("WARN", "任務已在執行中") return self.tasks.clear() @@ -132,31 +136,34 @@ class MissionExecutor(QObject): f"rendezvous WP={sorted(self.rendezvous_indices)}" if self.rendezvous_indices else "無 rendezvous (各自跑)" ) - print(f"任務啟動: {len(self.tasks)} 架無人機, " - f"共 {total_wps} 個航點, " - f"到達半徑={self.arrival_radius}m, " - f"tick 週期={self._interval_ms}ms, " - f"barrier timeout={self.barrier_timeout_sec}s, " - f"{rv_info}") + _log( + "INFO", + f"任務已啟動: {len(self.tasks)} 架無人機, " + f"共 {total_wps} 個航點, " + f"到達半徑={self.arrival_radius}m, " + f"tick 週期={self._interval_ms}ms, " + f"barrier timeout={self.barrier_timeout_sec}s, " + f"{rv_info}", + ) def pause(self): if self.state == MissionState.RUNNING: self._timer.stop() self.state = MissionState.PAUSED - print("任務暫停") + _log("INFO", "任務已暫停") def resume(self): if self.state == MissionState.PAUSED: self._timer.start(self._interval_ms) self.state = MissionState.RUNNING - print("任務恢復") + _log("INFO", "任務已恢復") def stop(self): self._timer.stop() self.tasks.clear() self.rendezvous_indices = set() self.state = MissionState.IDLE - print("任務停止") + _log("INFO", "任務已停止") # ------------------------------------------------------------------ 控制迴圈 @@ -197,7 +204,7 @@ class MissionExecutor(QObject): # rendezvous 點 → 不推進,進入 barrier 等待 task.status = TaskStatus.WAITING_AT_BARRIER task.waiting_since = now - print(f" {task.drone_id} → 抵達 barrier WP {task.wp_index},等待同伴") + _log("INFO", f"{task.drone_id} 抵達 barrier WP {task.wp_index},等待同伴") self.task_status_changed.emit( task.drone_id, task.status.value, f"waiting at WP {task.wp_index}" @@ -236,7 +243,7 @@ class MissionExecutor(QObject): self._timer.stop() self.state = MissionState.IDLE self.mission_completed.emit() - print("===== 任務全部完成 =====") + _log("INFO", "任務全部完成") def _advance_waypoint(self, task, arrived_distance): """把 task 推進一個航點,重置發送旗標。不處理 barrier 邏輯。""" @@ -248,14 +255,20 @@ class MissionExecutor(QObject): self.drone_waypoint_reached.emit( task.drone_id, task.wp_index, task.total_waypoints ) - print(f" {task.drone_id} → DONE " - f"({task.total_waypoints}/{task.total_waypoints})") + _log( + "INFO", + f"{task.drone_id} 已完成所有航點 " + f"({task.total_waypoints}/{task.total_waypoints})", + ) return self.drone_waypoint_reached.emit( task.drone_id, task.wp_index, task.total_waypoints ) - print(f" {task.drone_id} → WP {task.wp_index}/{task.total_waypoints} " - f"(到達距離: {arrived_distance:.1f}m)") + _log( + "INFO", + f"{task.drone_id} 前往 WP {task.wp_index}/{task.total_waypoints} " + f"(到達距離: {arrived_distance:.1f}m)", + ) def _check_barriers(self, now): """檢查每個有人在等的 barrier 能不能釋放。""" @@ -289,14 +302,13 @@ class MissionExecutor(QObject): oldest_wait = min(t.waiting_since for t in waiting_tasks) if now - oldest_wait >= self.barrier_timeout_sec: force_reason = f"timeout {self.barrier_timeout_sec:.0f}s" - print(f"⚠️ barrier WP {barrier_idx} {force_reason},強制放行") + _log("WARN", f"barrier WP {barrier_idx} {force_reason},強制放行") else: continue # 還沒到齊、也還沒 timeout → 繼續等 # 釋放:把所有在此 barrier 等待的機一起推進 tag = "全員到齊" if force_reason is None else force_reason - print(f"✅ barrier WP {barrier_idx} 釋放 ({tag})," - f"推進 {len(waiting_tasks)} 架") + _log("INFO", f"barrier WP {barrier_idx} 已釋放 ({tag}),推進 {len(waiting_tasks)} 架") for task in waiting_tasks: task.status = TaskStatus.NORMAL msg = f"barrier WP {barrier_idx} released ({tag})" @@ -324,7 +336,7 @@ class MissionExecutor(QObject): return task.fail_count += 1 - print(f"⚠️ {drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}") + _log("WARN", f"{drone_id} 發送失敗 {task.fail_count}/{self.MAX_RETRY}: {message}") if task.fail_count < self.MAX_RETRY: task.status = TaskStatus.RETRYING @@ -339,13 +351,13 @@ class MissionExecutor(QObject): drone_id, task.status.value, f"fallback LOITER after {self.MAX_RETRY} fails: {message}" ) - print(f"❌ {drone_id} 連續失敗 {self.MAX_RETRY} 次 → 切換 LOITER") + _log("ERROR", f"{drone_id} 連續失敗 {self.MAX_RETRY} 次,切換至 LOITER") self._fallback_to_loiter(drone_id) def _fallback_to_loiter(self, drone_id): """用 monitor.set_mode 切 LOITER。set_mode 是 coroutine,透過 event loop 派送。""" if self.monitor is None: - print(f"⚠️ 無 monitor,無法將 {drone_id} 切到 LOITER") + _log("WARN", f"無 monitor,無法將 {drone_id} 切換至 LOITER") return try: loop = asyncio.get_event_loop()