2.7.0 logs

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)
 
@@ -801,6 +813,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 開始）。"""
         with self.socket_id_lock:
@@ -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, ...
@@ -1041,6 +1089,21 @@ class DroneMonitor(Node):
                 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:

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('_')

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"

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

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 大量互動 也許需要考慮對方的生命週期
 
 '''
-

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']
+__all__ = ['RingBuffer', 'get_ros_log_queue', 'setup_logger']

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 名稱