From f1413111732ebebc974ef700dc04c20fb65ed735 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Mon, 6 Jul 2026 07:29:56 +0800 Subject: [PATCH] 2.7.0 logs --- src/.codex | 0 src/GUI/communication.py | 124 ++++++++++ src/GUI/gui.py | 215 +++++++++++++++--- src/fc_interfaces/CMakeLists.txt | 1 + src/fc_interfaces/msg/FcNetworkLog.msg | 12 + .../fc_network_adapter/mavlinkROS2Nodes.py | 77 ++++++- .../fc_network_adapter/utils/__init__.py | 4 +- .../fc_network_adapter/utils/theLogger.py | 42 ++++ 8 files changed, 442 insertions(+), 33 deletions(-) create mode 100644 src/.codex create mode 100644 src/fc_interfaces/msg/FcNetworkLog.msg diff --git a/src/.codex b/src/.codex new file mode 100644 index 0000000..e69de29 diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 913a5a8..25365fa 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -69,6 +69,18 @@ except ImportError as e: _log("ERROR", f"錯誤: {e}") GnssRaw = None +try: + from fc_interfaces.msg import SystemDiagnosticsRaw +except ImportError as e: + _log("WARN", f"SystemDiagnosticsRaw 尚不可用，略過 sys_diags 訂閱: {e}") + SystemDiagnosticsRaw = None + +try: + from fc_interfaces.msg import FcNetworkLog +except ImportError as e: + _log("WARN", f"FcNetworkLog 尚不可用，略過 FC Network 紀錄訂閱: {e}") + FcNetworkLog = None + class DroneSignals(QObject): update_signal = pyqtSignal(str, str, object) # (msg_type, drone_id, data) @@ -800,6 +812,16 @@ class DroneMonitor(Node): # 主题检测定时器 self.create_timer(1.0, self.scan_topics) + + # FC Network 使用專用結構化紀錄 topic，不再解析 /rosout。 + self.fc_network_log_sub = None + if FcNetworkLog is not None: + self.fc_network_log_sub = self.create_subscription( + FcNetworkLog, + '/fc_network/logs', + self.fc_network_log_callback, + 200 + ) def get_next_socket_id(self): """取得目前最小的未使用 socket_id（從 0 開始）。""" @@ -969,6 +991,32 @@ class DroneMonitor(Node): setattr(self, subs_attr, subs) except Exception: pass + if isinstance(subs, dict) and 'sys_diags' not in subs and SystemDiagnosticsRaw is not None: + base_topic = f'/fc_network/vehicle/{sys_id}' + try: + sys_diags_sub = self.create_subscription( + SystemDiagnosticsRaw, + f'{base_topic}/sys_diags', + lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg), + 10 + ) + subs['sys_diags'] = sys_diags_sub + setattr(self, subs_attr, subs) + except Exception: + pass + if isinstance(subs, dict) and 'status_text' not in subs: + base_topic = f'/fc_network/vehicle/{sys_id}' + try: + status_text_sub = self.create_subscription( + String, + f'{base_topic}/status_text', + lambda msg, sid=sys_id: self.status_text_callback(sid, msg), + 10 + ) + subs['status_text'] = status_text_sub + setattr(self, subs_attr, subs) + except Exception: + pass def setup_drone(self, sys_id): # sys_id 格式： sys11, sys12, ... @@ -1040,6 +1088,21 @@ class DroneMonitor(Node): lambda msg, sid=sys_id: self.attitude_callback(sid, msg), 10 ) + + if SystemDiagnosticsRaw is not None: + subs['sys_diags'] = self.create_subscription( + SystemDiagnosticsRaw, + f'{base_topic}/sys_diags', + lambda msg, sid=sys_id: self.sys_diags_callback(sid, msg), + 10 + ) + + subs['status_text'] = self.create_subscription( + String, + f'{base_topic}/status_text', + lambda msg, sid=sys_id: self.status_text_callback(sid, msg), + 10 + ) setattr(self, f'drone_{sys_id}_subs', subs) @@ -1299,6 +1362,67 @@ class DroneMonitor(Node): 'voltage': msg.voltage } + def sys_diags_callback(self, sys_id, msg): + """轉送 /fc_network/vehicle/sysN/sys_diags 到 GUI 紀錄區。""" + stamp = getattr(msg, 'stamp', None) + data = { + 'stamp': { + 'sec': getattr(stamp, 'sec', 0), + 'nanosec': getattr(stamp, 'nanosec', 0), + }, + 'sensors_install_mask': int(msg.sensors_install_mask), + 'sensors_enabled_mask': int(msg.sensors_enabled_mask), + 'sensors_health_mask': int(msg.sensors_health_mask), + 'mcu_load': int(msg.mcu_load), + 'mcu_load_percent': float(msg.mcu_load) / 10.0, + 'bus_error_rate': int(msg.bus_error_rate), + 'bus_error_rate_percent': float(msg.bus_error_rate) / 10.0, + 'bus_error_count': int(msg.bus_error_count), + 'errors_count1': int(msg.errors_count1), + 'errors_count2': int(msg.errors_count2), + 'errors_count3': int(msg.errors_count3), + 'errors_count4': int(msg.errors_count4), + } + self.signals.update_signal.emit('sys_diags', sys_id, data) + + def status_text_callback(self, sys_id, msg): + """轉送飛控 STATUSTEXT（飛行檢查、警告與錯誤）。""" + raw = msg.data.strip() + if not raw: + return + + match = re.match(r'^\[([\d.]+)\]\s*\[(-?\d+)\]\s*(.*)$', raw) + if match: + vehicle_timestamp = float(match.group(1)) + severity = int(match.group(2)) + text = match.group(3) + else: + vehicle_timestamp = None + severity = -1 + text = raw + + self.signals.update_signal.emit('status_text', sys_id, { + 'severity': severity, + 'text': text, + 'vehicle_timestamp': vehicle_timestamp, + 'raw': raw, + }) + + def fc_network_log_callback(self, msg): + """接收 /fc_network/logs 的結構化紀錄。""" + stamp = getattr(msg, 'stamp', None) + self.signals.update_signal.emit('fc_network_log', msg.source or 'fc_network', { + 'stamp': { + 'sec': getattr(stamp, 'sec', 0), + 'nanosec': getattr(stamp, 'nanosec', 0), + }, + 'level': int(msg.level), + 'source': msg.source, + 'event_code': msg.event_code, + 'sysid': int(msg.sysid), + 'message': msg.message, + }) + def state_callback(self, drone_id, msg): mode = msg.mode if mode in self.filtered_modes: diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1b72e67..fd73dbd 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -18,11 +18,9 @@ import re import threading from concurrent.futures import ThreadPoolExecutor - def _log(level, message): print(f"[{level}] {message}", flush=True) - # 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver from map_layout import DroneMap @@ -148,7 +146,7 @@ class ToggleSwitch(QWidget): class ControlStationUI(QMainWindow): planning_finished = pyqtSignal(object) - VERSION = '2.6.0' + VERSION = '2.7.0' FONT_SCALE_MIN = 70 FONT_SCALE_MAX = 180 FONT_SCALE_DEFAULT = 100 @@ -211,8 +209,15 @@ class ControlStationUI(QMainWindow): self._attitude_cache = {} self._overview_cache = {} self._map_dirty_drones = set() - self.message_history = [] + # 「紀錄」分頁分成三個獨立來源；message_history 保留為 GUI 操作紀錄的 + # 相容別名，避免既有程式仍存取舊屬性時失效。 + self.drone_message_history = [] + self.gui_operation_history = [] + self.fcnetwork_history = [] + self.message_history = self.gui_operation_history self.max_message_history = 500 + self._sys_diags_warning_signatures = {} + self._suppressed_status_message = None # 初始化UI self.drones = {} @@ -448,19 +453,19 @@ class ControlStationUI(QMainWindow): 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 = QLabel("訊息與操作紀錄") title.setStyleSheet("color: #DDD; font-size: 14px; font-weight: bold;") header_layout.addWidget(title) header_layout.addStretch() - clear_btn = QPushButton("清空") + clear_btn = QPushButton("全部清空") clear_btn.setStyleSheet(""" QPushButton { background-color: #555; color: white; border: none; padding: 5px 10px; border-radius: 4px; font-size: 12px; } @@ -470,9 +475,7 @@ class ControlStationUI(QMainWindow): 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(""" + history_style = """ QPlainTextEdit { background-color: #1E1E1E; color: #DDD; @@ -482,9 +485,67 @@ class ControlStationUI(QMainWindow): font-family: monospace; font-size: 12px; } - """) - self.message_history_view.setPlaceholderText("左下角狀態訊息會顯示在這裡...") - layout.addWidget(self.message_history_view) + """ + + def create_section(section_title, category): + section = QWidget() + section_layout = QVBoxLayout(section) + section_layout.setContentsMargins(0, 0, 0, 0) + section_layout.setSpacing(4) + + section_header = QHBoxLayout() + section_label = QLabel(section_title) + section_label.setStyleSheet( + "color: #CCC; font-size: 12px; font-weight: bold;") + section_header.addWidget(section_label) + section_header.addStretch() + + section_clear_btn = QPushButton("清空") + section_clear_btn.setStyleSheet(""" + QPushButton { background-color: #444; color: #DDD; border: none; + padding: 3px 8px; border-radius: 3px; font-size: 11px; } + QPushButton:hover { background-color: #555; } + """) + section_clear_btn.clicked.connect( + lambda _checked=False, name=category: + self._clear_message_history(name) + ) + section_header.addWidget(section_clear_btn) + section_layout.addLayout(section_header) + + view = QPlainTextEdit() + view.setReadOnly(True) + view.setStyleSheet(history_style) + view.document().setMaximumBlockCount(self.max_message_history) + section_layout.addWidget(view) + return section, view + + history_splitter = QSplitter(Qt.Orientation.Vertical) + history_splitter.setChildrenCollapsible(False) + + gui_section, self.gui_operation_history_view = create_section( + "操作紀錄", "gui") + drone_section, self.drone_message_history_view = create_section( + "無人機狀態", "drone") + fcnetwork_section, self.fcnetwork_history_view = create_section( + "FC Network", "fcnetwork") + + history_splitter.addWidget(gui_section) + history_splitter.addWidget(drone_section) + history_splitter.addWidget(fcnetwork_section) + history_splitter.setStretchFactor(0, 1) + history_splitter.setStretchFactor(1, 1) + history_splitter.setStretchFactor(2, 1) + history_splitter.setSizes([220, 220, 220]) + layout.addWidget(history_splitter) + + self._history_views = { + 'drone': self.drone_message_history_view, + 'gui': self.gui_operation_history_view, + 'fcnetwork': self.fcnetwork_history_view, + } + # 舊名稱指向 GUI 操作紀錄，保留向後相容性。 + self.message_history_view = self.gui_operation_history_view return widget @@ -741,27 +802,114 @@ class ControlStationUI(QMainWindow): return super().eventFilter(obj, event) - def _clear_message_history(self): - """清空訊息歷史。""" - self.message_history.clear() - if hasattr(self, 'message_history_view'): - self.message_history_view.clear() + def _history_list(self, category): + return { + 'drone': self.drone_message_history, + 'gui': self.gui_operation_history, + 'fcnetwork': self.fcnetwork_history, + }.get(category) - def _on_status_bar_message_changed(self, message): - """同步狀態列訊息到歷史紀錄。""" + def _append_history(self, category, 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:] + history = self._history_list(category) + if history is None: + return + history.append(entry) + if len(history) > self.max_message_history: + del history[:-self.max_message_history] - if hasattr(self, 'message_history_view'): - self.message_history_view.appendPlainText(entry) - scrollbar = self.message_history_view.verticalScrollBar() + view = getattr(self, '_history_views', {}).get(category) + if view is not None: + view.appendPlainText(entry) + scrollbar = view.verticalScrollBar() scrollbar.setValue(scrollbar.maximum()) + def _clear_message_history(self, category=None): + """清空單一來源；未指定來源時清空全部。""" + categories = (category,) if category else ('drone', 'gui', 'fcnetwork') + for name in categories: + history = self._history_list(name) + if history is not None: + history.clear() + view = getattr(self, '_history_views', {}).get(name) + if view is not None: + view.clear() + + def _record_drone_status(self, sys_id, data): + """顯示由 status_text topic 收到的飛行檢查與報錯。""" + severity_labels = { + 0: 'EMERGENCY', 1: 'ALERT', 2: 'CRITICAL', 3: 'ERROR', + 4: 'WARNING', 5: 'NOTICE', 6: 'INFO', 7: 'DEBUG' + } + severity = data.get('severity', -1) + label = severity_labels.get(severity, 'STATUS') + self._append_history( + 'drone', f"/fc_network/vehicle/{sys_id}/status_text " + f"[{label}] {data.get('text', '')}") + + def _record_sys_diags_warning(self, sys_id, data): + """sys_diags 只在診斷值異常或內容改變時顯示警告。""" + installed = int(data.get('sensors_install_mask', 0)) + enabled = int(data.get('sensors_enabled_mask', 0)) + healthy = int(data.get('sensors_health_mask', 0)) + unhealthy_mask = installed & enabled & ~healthy + load = float(data.get('mcu_load_percent', 0.0)) + bus_rate = float(data.get('bus_error_rate_percent', 0.0)) + error_counts = tuple(int(data.get(f'errors_count{i}', 0)) for i in range(1, 5)) + + warnings = [] + if unhealthy_mask: + warnings.append(f"感測器異常 mask=0x{unhealthy_mask:08X}") + if load >= 90.0: + warnings.append(f"MCU 負載過高 {load:.1f}%") + if bus_rate > 0.0 or int(data.get('bus_error_count', 0)) > 0: + warnings.append( + f"匯流排錯誤率 {bus_rate:.1f}% / " + f"count={int(data.get('bus_error_count', 0))}") + if any(error_counts): + warnings.append(f"錯誤計數={error_counts}") + + signature = tuple(warnings) + if not signature: + self._sys_diags_warning_signatures.pop(sys_id, None) + return + if self._sys_diags_warning_signatures.get(sys_id) == signature: + return + self._sys_diags_warning_signatures[sys_id] = signature + self._append_history( + 'drone', f"/fc_network/vehicle/{sys_id}/sys_diags [WARNING] " + + "；".join(warnings)) + + def _record_fcnetwork_log(self, logger_name, data): + """顯示 /fc_network/logs 的結構化 FC Network 紀錄。""" + level_labels = { + 10: 'DEBUG', 20: 'INFO', 30: 'WARN', 40: 'ERROR', 50: 'FATAL' + } + level = level_labels.get(data.get('level'), str(data.get('level', ''))) + context = [] + if data.get('event_code'): + context.append(data['event_code']) + if data.get('sysid', -1) >= 0: + context.append(f"sys{data['sysid']}") + context_text = f" [{' / '.join(context)}]" if context else '' + self._append_history( + 'fcnetwork', f"/fc_network/logs [{level}] [{logger_name}]" + f"{context_text} {data.get('message', '')}") + + def _on_status_bar_message_changed(self, message): + """同步 GUI 狀態列訊息到操作紀錄。""" + if not message: + return + if message == self._suppressed_status_message: + self._suppressed_status_message = None + return + self._append_history('gui', message) + def _setup_stream_redirector(self): """將 stdout/stderr 同步到左下角狀態列與訊息紀錄。""" self._original_stdout = sys.stdout @@ -783,10 +931,14 @@ class ControlStationUI(QMainWindow): sys.stderr = self._original_stderr def show_in_bottom_left(self, text): - """將重導向的輸出顯示在左下角狀態列。""" + """背景輸出只短暫顯示在狀態列，不混入三類紀錄。""" if not text: return + # 背景輸出仍沿用既有狀態列提示，但避免又被當成 GUI 操作紀錄。 + self._suppressed_status_message = text self.statusBar().showMessage(text, 5000) + if self._suppressed_status_message == text: + self._suppressed_status_message = None # ================================================================================ @@ -1660,6 +1812,15 @@ class ControlStationUI(QMainWindow): def update_ui(self, msg_type, drone_id, data): """只做數據快取，不在這裡更新 UI""" + if msg_type == 'status_text': + self._record_drone_status(drone_id, data) + return + if msg_type == 'sys_diags': + self._record_sys_diags_warning(drone_id, data) + return + if msg_type == 'fc_network_log': + self._record_fcnetwork_log(drone_id, data) + return if msg_type == 'connection_type': conn_type = data.get('type', 'Unknown') parts = drone_id.split('_') diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt index 906d9c9..87400b2 100644 --- a/src/fc_interfaces/CMakeLists.txt +++ b/src/fc_interfaces/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/AttitudeRaw.msg" "msg/GnssRaw.msg" "msg/SystemDiagnosticsRaw.msg" + "msg/FcNetworkLog.msg" "msg/ServiceAckResult.msg" "srv/MavPing.srv" "srv/MavCommandLong.srv" diff --git a/src/fc_interfaces/msg/FcNetworkLog.msg b/src/fc_interfaces/msg/FcNetworkLog.msg new file mode 100644 index 0000000..e4e2a13 --- /dev/null +++ b/src/fc_interfaces/msg/FcNetworkLog.msg @@ -0,0 +1,12 @@ +uint8 DEBUG=10 +uint8 INFO=20 +uint8 WARN=30 +uint8 ERROR=40 +uint8 FATAL=50 + +builtin_interfaces/Time stamp +uint8 level +string source +string event_code +int32 sysid +string message diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 7f080a5..2522a5d 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -1,6 +1,6 @@ """ MAVLink ROS2 Nodes -主要包含三個獨立的 ROS2 Node : +主要包含四個獨立的 ROS2 Node : 1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics 從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計 2. MavlinkCommandService - 提供 MAVLink 指令 service 介面 @@ -8,6 +8,7 @@ MAVLink ROS2 Nodes 並不會包含額外的功能 3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具 過期丟棄、去重、節流、分片 +4. FcNetworkLogPublisher - 將 FC Network logging queue 發布到 /fc_network/logs 與一個節點管理器 - fc_ros_manager @@ -18,6 +19,7 @@ import os import time import math import hashlib +import queue import threading from typing import Dict, Optional @@ -42,7 +44,7 @@ from fc_interfaces.msg import ServiceAckResult # 自定義 imports from . import mavlinkVehicleView as mvv from . import mavlinkObject as mo -from .utils import setup_logger +from .utils import get_ros_log_queue, setup_logger logger = setup_logger(os.path.basename(__file__)) MODULE_VER = "2.50" @@ -1179,6 +1181,58 @@ class RtcmRelay(Node): # logger.info("RtcmRelay stopped") +# ============================================================================ +# FC Network Log Publisher Node +# ============================================================================ + +class FcNetworkLogPublisher(Node): + """從 Python logging queue 發布結構化 /fc_network/logs 訊息。""" + + def __init__(self): + super().__init__('fc_network_log_publisher') + qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=200, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ) + self.publisher = self.create_publisher( + fcmsg.FcNetworkLog, '/fc_network/logs', qos) + self.log_queue = get_ros_log_queue() + self.running = True + self.timer = self.create_timer(0.05, self._drain_queue) + + def _drain_queue(self): + if not self.running: + return + + for _ in range(100): + try: + item = self.log_queue.get_nowait() + except queue.Empty: + break + + created = max(0.0, float(item.get('created', 0.0))) + seconds = int(created) + nanoseconds = int((created - seconds) * 1_000_000_000) + + msg = fcmsg.FcNetworkLog() + msg.stamp.sec = seconds + msg.stamp.nanosec = min(max(nanoseconds, 0), 999_999_999) + msg.level = min(max(int(item.get('level', 20)), 0), 255) + msg.source = str(item.get('source', 'fc_network')) + msg.event_code = str(item.get('event_code', '')) + msg.sysid = min( + max(int(item.get('sysid', -1)), -2_147_483_648), + 2_147_483_647 + ) + msg.message = str(item.get('message', '')) + self.publisher.publish(msg) + + def stop(self): + self.running = False + + # ============================================================================ # ROS2 節點管理器 # ============================================================================ @@ -1192,10 +1246,11 @@ class fc_ros_manager: stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。 shutdown 是完全關閉 ROS2 並銷毀節點實例。 - 管理三個獨立的 ROS2 Node : + 管理四個獨立的 ROS2 Node : - VehicleStatusPublisher - MavlinkCommandService - RtcmRelay + - FcNetworkLogPublisher 提供統一的啟動/停止介面給 mainOrchestrator @@ -1215,6 +1270,7 @@ class fc_ros_manager: self.status_publisher: Optional[VehicleStatusPublisher] = None self.command_service: Optional[MavlinkCommandService] = None self.rtcm_relay: Optional[RtcmRelay] = None + self.log_publisher: Optional[FcNetworkLogPublisher] = None # Executor & Thread self.spin_thread: Optional[threading.Thread] = None @@ -1235,12 +1291,19 @@ class fc_ros_manager: self.status_publisher = VehicleStatusPublisher() self.command_service = MavlinkCommandService() self.rtcm_relay = RtcmRelay() + if hasattr(fcmsg, 'FcNetworkLog'): + self.log_publisher = FcNetworkLogPublisher() + else: + logger.warning( + "FcNetworkLog interface unavailable; /fc_network/logs disabled") # 創建執行者 MultiThreadedExecutor 並把 node 加入其中 self.executor = MultiThreadedExecutor() self.executor.add_node(self.status_publisher) self.executor.add_node(self.command_service) self.executor.add_node(self.rtcm_relay) + if self.log_publisher: + self.executor.add_node(self.log_publisher) self.initialized = True # logger.info("fc_ros_manager initialized") @@ -1266,6 +1329,8 @@ class fc_ros_manager: self.status_publisher.running = True self.command_service.running = True self.rtcm_relay.running = True + if self.log_publisher: + self.log_publisher.running = True self.spin_thread = threading.Thread( target=self._spin_executor, @@ -1391,6 +1456,8 @@ class fc_ros_manager: self.command_service.stop() if self.rtcm_relay: self.rtcm_relay.stop() + if self.log_publisher: + self.log_publisher.stop() # 等待 spin 執行緒結束 if self.spin_thread and self.spin_thread.is_alive(): @@ -1417,6 +1484,8 @@ class fc_ros_manager: self.command_service.destroy_node() if self.rtcm_relay: self.rtcm_relay.destroy_node() + if self.log_publisher: + self.log_publisher.destroy_node() # 關閉 ROS2 if rclpy.ok(): @@ -1435,6 +1504,7 @@ class fc_ros_manager: 'status_publisher_active': self.status_publisher is not None and self.status_publisher.running, 'command_service_active': self.command_service is not None, 'rtcm_relay_active': self.rtcm_relay is not None and self.rtcm_relay.running, + 'log_publisher_active': self.log_publisher is not None and self.log_publisher.running, } @@ -1489,4 +1559,3 @@ TODO 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 ''' - diff --git a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py index 921faf4..a6d208d 100644 --- a/src/fc_network_adapter/fc_network_adapter/utils/__init__.py +++ b/src/fc_network_adapter/fc_network_adapter/utils/__init__.py @@ -2,6 +2,6 @@ 共用工具模組 """ from .ringBuffer import RingBuffer -from .theLogger import setup_logger +from .theLogger import get_ros_log_queue, setup_logger -__all__ = ['RingBuffer', 'setup_logger'] \ No newline at end of file +__all__ = ['RingBuffer', 'get_ros_log_queue', 'setup_logger'] diff --git a/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py index dce2ce7..500644f 100644 --- a/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py +++ b/src/fc_network_adapter/fc_network_adapter/utils/theLogger.py @@ -1,9 +1,47 @@ import logging import os +import queue from logging.handlers import TimedRotatingFileHandler # 全域 Logger 實例 _global_logger = None +_ros_log_queue = queue.Queue(maxsize=2000) + + +class RosTopicQueueHandler(logging.Handler): + """將 FC Network log record 非阻塞地排入 ROS publisher 佇列。""" + + def emit(self, record: logging.LogRecord) -> None: + try: + sysid = int(getattr(record, 'sysid', -1)) + except (TypeError, ValueError): + sysid = -1 + + item = { + 'created': float(record.created), + 'level': int(record.levelno), + 'source': str(record.name), + 'event_code': str(getattr(record, 'event_code', '')), + 'sysid': sysid, + 'message': record.getMessage(), + } + try: + _ros_log_queue.put_nowait(item) + except queue.Full: + # GUI 不應反過來拖慢通訊；滿載時淘汰最舊一筆。 + try: + _ros_log_queue.get_nowait() + except queue.Empty: + return + try: + _ros_log_queue.put_nowait(item) + except queue.Full: + pass + + +def get_ros_log_queue(): + """供 ROS publisher node 取得共用、thread-safe 的紀錄佇列。""" + return _ros_log_queue def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> logging.Logger: global _global_logger @@ -37,6 +75,10 @@ def setup_logger(name: str, log_dir: str = "logs", level=logging.DEBUG) -> loggi console_handler.setFormatter(formatter) _global_logger.addHandler(console_handler) + ros_queue_handler = RosTopicQueueHandler() + ros_queue_handler.setLevel(logging.INFO) + _global_logger.addHandler(ros_queue_handler) + # 為每個模組建立子 Logger，並設定名稱 module_logger = _global_logger.getChild(name) module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱