2.7.0 logs
This commit is contained in:
@@ -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 大量互動 也許需要考慮對方的生命週期
|
||||
|
||||
'''
|
||||
|
||||
|
||||
@@ -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']
|
||||
|
||||
@@ -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 名稱
|
||||
|
||||
Reference in New Issue
Block a user