chiyu1468/AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

# RTK function update

Browse Source 
---
1. mavlinkROS2Node 新增 RtcmRelay Node 監聽 RTCM topic 並轉發到 mavlinkObject bus (GPS_RTCM_DATA)
2. 新增 fc_network_module ros2 package 作為協同附屬模組的放置空間
3. Create a new fc_network_module with NtripClientNode for connecting to NTRIP casters and publishing RTCM data to ROS2 topics.
4. (modify) mainOrchestrator adjust the publishing rates for GNSS topics
This commit is contained in:
chiyu1468
2026-05-13 18:14:47 +08:00
parent c8f070bfb9
commit 5c14c45823
14 changed files with 519 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md
+19 -2
View File
@@ -176,13 +176,15 @@
這裡支持了所有 ROS2 框架的對應功能 包涵
1. 將載具的串流更新的狀態更新到對應的 topic
2. 將地面站與載具互動 mavlink microservice 包裝起來提供 ros2 service 互動介面
3. 訂閱外部 RTCM 資料並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
### **[Class]** fc_ros_manager
MAVLink ROS2 節點管理器 管理兩個獨立的 Node
會開一個執行緒 讓兩個 Node 都跑在裡面
MAVLink ROS2 節點管理器 管理三個獨立的 Node
會開一個執行緒 讓三個 Node 都跑在裡面
然後用 spin_once 保持 Node 的活性
- *self.status_publisher* 物件實例
- *self.command_service* 物件實例
- *self.rtcm_relay* 物件實例
- *self.spin_thread* 執行緒
---
- *start()* 啟動自己
@@ -210,6 +212,20 @@
- *__init__()* 這邊登入要創建的 service 到 ros2 系統
- *handle_XXX()* 這邊是實現 service 的具體方法
### **[Class]** RtcmRelay
訂閱 /fc_network/rtcm_in (mavros_msgs/msg/RTCM) 並轉發為 MAVLink GPS_RTCM_DATA
topic 的 QoS：BEST_EFFORT / KEEP_LAST depth=1 / VOLATILE（低延遲優先，丟舊包不排隊）
處理順序：過期丟棄(1s) → blake2s hash 去重 → 最短轉發間隔節流 → 分片轉發
分片：GPS_RTCM_DATA 每片 180 bytes，超過自動分片，最多 4 片 (720 bytes)，超長直接丟棄
失敗語意：不做舊包重送，送失敗就放棄等下一包
- *self.counters* 觀測用計數器 (received/forwarded/dropped_stale/dropped_duplicate/dropped_throttle/dropped_oversize/no_route/send_fail)
- *self.clock_offset_sec* 與 RTCM 來源的時鐘偏移量（秒）
---
- *_rtcm_callback()* 收到 RTCM 封包時的處理流程
- *_send_rtcm()* 對單一載具編碼並送出 GPS_RTCM_DATA（含分片邏輯）
- *set_clock_offset()* 設定時鐘偏移量
- *get_counters()* 取得觀測計數器
# 開發記錄
@@ -225,6 +241,7 @@
9. 載具狀態收集與彙整
10. a. ros2 topic 應用開發介面 (基礎框架)
b. ros2 service 應用開發介面 (基礎框架)
11. RTCM 轉發 - 訂閱 RTCM topic 轉為 MAVLink GPS_RTCM_DATA 廣播給所有載具
### 待開發功能
5-1. 建立 serial 連線 並可以對接收器下達AT指令
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+3 -3
View File
@@ -1145,9 +1145,9 @@ class Orchestrator:
self.fc_ros_manager = mros.ros2_manager
self.fc_ros_manager.initialize()
self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
'position_gnss': 1.0,
'position_ned': 1.0,
'attitude': 1.0,
'position_gnss': 0.2,
'position_ned': 0.5,
'attitude': 0.1,
'velocity': 0.0,
'battery': 1.0,
'vfr_hud': 1.0,
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+197 -6
View File
@@ -1,11 +1,13 @@
"""
MAVLink ROS2 Nodes
主要包含兩個獨立的 ROS2 Node :
主要包含三個獨立的 ROS2 Node :
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面
以最基礎的 mavlink microservice 會實現目標
並不會包含額外的功能
3. RtcmRelay - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
過期丟棄、去重、節流、分片
與一個節點管理器
- fc_ros_manager
@@ -15,6 +17,7 @@ MAVLink ROS2 Nodes
import os
import time
import math
import hashlib
import threading
from typing import Dict, Optional
@@ -22,6 +25,7 @@ from typing import Dict, Optional
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
# ROS2 Message imports
import std_msgs.msg
@@ -131,8 +135,8 @@ class VehicleStatusPublisher(Node):
self.fc_publishers: Dict[tuple, any] = {}
# 定時器：以較高頻率檢查 vehicle_registry 並發布
# 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.1 # 100ms
# 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
self.timer_period = 0.05 # 50ms
self.timer = self.create_timer(self.timer_period, self.timer_callback)
# 狀態標誌
@@ -436,7 +440,7 @@ class VehicleStatusPublisher(Node):
# 'longitude': status.position.longitude if status.position.longitude else 0.0,
# 'altitude': status.position.altitude if status.position.altitude else 0.0,
# 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
# 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
'connection_type': vehicle.connected_via.value,
'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
}
@@ -955,6 +959,179 @@ class MavlinkCommandService(Node):
logger.info("MavlinkCommandService stopped")
# ============================================================================
# RtcmRelay Node
# ============================================================================
class RtcmRelay(Node):
"""
RTCM 轉發節點 - 訂閱 RTCM topic 並轉發為 MAVLink GPS_RTCM_DATA 給所有載具
職責:
- 訂閱外部 RTCM 資料（mavros_msgs/msg/RTCM）
- 過期丟棄：封包 Header.stamp 超過 1 秒則丟棄
- 去重：與上一筆 payload blake2s hash 相同則丟棄
- 節流：最短轉發間隔保護（可參數化）
- 對所有在線載具直接編碼並送出 MAVLink GPS_RTCM_DATA（含分片）
MAVLink GPS_RTCM_DATA (msg_id=233) 每片 payload 上限 180 bytes
超過 180 bytes 需分片，最多 4 片 = 720 bytes
"""
FRAGMENT_MAX_LEN = 180
MAX_TOTAL_LEN = 4 * 180 # 720 bytes
STALE_THRESHOLD_SEC = 1.0
def __init__(self, min_forward_interval_ms: float = 0.0, clock_offset_ms: float = 0.0):
super().__init__('rtcm_relay')
self.running = True
self.min_forward_interval_sec = min_forward_interval_ms / 1000.0
self.clock_offset_sec = clock_offset_ms / 1000.0
# 去重：上一筆 payload 的短 hash
self._last_hash: Optional[bytes] = None
# 節流：上次成功轉發的時間
self._last_forward_time: float = 0.0
# MAVLink GPS_RTCM_DATA 分片序號 (0~31 循環，同一筆 RTCM 的所有分片共用)
self._seq: int = 0
# 觀測 counters
self.counters = {
'received': 0,
'forwarded': 0,
'dropped_stale': 0,
'dropped_duplicate': 0,
'dropped_throttle': 0,
'dropped_oversize': 0,
'no_route': 0,
'send_fail': 0,
}
# QoS: 低延遲、弱可靠 — 寧可掉舊包也不排隊
rtcm_qos = QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE,
)
self.subscription = self.create_subscription(
mavros_msgs.msg.RTCM,
'/fc_network/rtcm_input',
self._rtcm_callback,
rtcm_qos,
)
logger.info("RtcmRelay initialized")
# ── callback ──────────────────────────────────────────────────────
def _rtcm_callback(self, msg):
"""處理順序：過期檢查 → 去重 → 節流 → 轉發"""
if not self.running:
return
self.counters['received'] += 1
data = bytes(msg.data)
now = time.time()
# 1) 過期丟棄
stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
age = (now + self.clock_offset_sec) - stamp_sec
if age > self.STALE_THRESHOLD_SEC:
self.counters['dropped_stale'] += 1
return
# 2) 去重：blake2s 短 hash 與上一筆比對
h = hashlib.blake2s(data, digest_size=8).digest()
if h == self._last_hash:
self.counters['dropped_duplicate'] += 1
return
self._last_hash = h
# 3) 節流：最短轉發間隔
if self.min_forward_interval_sec > 0:
if (now - self._last_forward_time) < self.min_forward_interval_sec:
self.counters['dropped_throttle'] += 1
return
# 4) 超長丟棄
if len(data) > self.MAX_TOTAL_LEN:
self.counters['dropped_oversize'] += 1
logger.warning(f"RTCM data too large ({len(data)} bytes), max {self.MAX_TOTAL_LEN}")
return
# 5) 取得本次的分片序號，然後遞增給下一筆
seq = self._seq
self._seq = (self._seq + 1) & 0x1F
# 6) 轉發給所有載具
forwarded_any = False
for sysid, vehicle in mvv.vehicle_registry.read_all():
socket_id = vehicle.custom_meta.get("socket_id")
if socket_id is None:
self.counters['no_route'] += 1
continue
mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id)
if mav_obj is None:
self.counters['no_route'] += 1
continue
try:
self._send_rtcm(mav_obj, data, seq)
forwarded_any = True
except Exception as e:
self.counters['send_fail'] += 1
logger.debug(f"RTCM send fail sysid={sysid}: {e}")
if forwarded_any:
self.counters['forwarded'] += 1
self._last_forward_time = now
# ── MAVLink 編碼與分片 ────────────────────────────────────────────
def _send_rtcm(self, mav_obj, data: bytes, seq: int):
"""
對單一載具送出 GPS_RTCM_DATA 必要時做分片
GPS_RTCM_DATA flags 欄位:
bit 0 : 1=fragmented
bit 1-2 : fragment_id (0~3)
bit 3-7 : sequence number (0~31)
"""
if len(data) <= self.FRAGMENT_MAX_LEN:
flags = (seq & 0x1F) << 3
padded = data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(data), padded)
else:
fragments = [
data[i:i + self.FRAGMENT_MAX_LEN]
for i in range(0, len(data), self.FRAGMENT_MAX_LEN)
]
for frag_id, frag_data in enumerate(fragments):
flags = 1 | ((frag_id & 0x03) << 1) | ((seq & 0x1F) << 3)
padded = frag_data + b'\x00' * (self.FRAGMENT_MAX_LEN - len(frag_data))
mav_obj.MAVLink.gps_rtcm_data_send(flags, len(frag_data), padded)
# ── 外部介面 ──────────────────────────────────────────────────────
def set_clock_offset(self, offset_ms: float):
"""更新時鐘偏移量（毫秒），用於與 RTCM 來源的時間對齊"""
self.clock_offset_sec = offset_ms / 1000.0
def get_counters(self) -> dict:
return dict(self.counters)
def stop(self):
"""停止轉發"""
self.running = False
logger.info("RtcmRelay stopped")
# ============================================================================
# ROS2 節點管理器
# ============================================================================
@@ -968,9 +1145,10 @@ class fc_ros_manager:
stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
shutdown 是完全關閉 ROS2 並銷毀節點實例。
管理兩個獨立的 ROS2 Node：
管理三個獨立的 ROS2 Node：
- VehicleStatusPublisher
- MavlinkCommandService
- RtcmRelay
提供統一的啟動/停止介面給 mainOrchestrator
"""
@@ -979,9 +1157,10 @@ class fc_ros_manager:
self.initialized = False
self.running = False
# 两个 node 實例
# node 實例
self.status_publisher: Optional[VehicleStatusPublisher] = None
self.command_service: Optional[MavlinkCommandService] = None
self.rtcm_relay: Optional[RtcmRelay] = None
# Executor & Thread
self.spin_thread: Optional[threading.Thread] = None
@@ -1001,11 +1180,13 @@ class fc_ros_manager:
# 創建節點 node
self.status_publisher = VehicleStatusPublisher()
self.command_service = MavlinkCommandService()
self.rtcm_relay = RtcmRelay()
# 創建執行者 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)
self.initialized = True
# logger.info("fc_ros_manager initialized")
@@ -1069,6 +1250,8 @@ class fc_ros_manager:
self.status_publisher.stop()
if self.command_service:
self.command_service.stop()
if self.rtcm_relay:
self.rtcm_relay.stop()
# 等待 spin 執行緒結束
if self.spin_thread and self.spin_thread.is_alive():
@@ -1093,6 +1276,8 @@ class fc_ros_manager:
self.status_publisher.destroy_node()
if self.command_service:
self.command_service.destroy_node()
if self.rtcm_relay:
self.rtcm_relay.destroy_node()
# 關閉 ROS2
if rclpy.ok():
@@ -1110,6 +1295,7 @@ class fc_ros_manager:
'running': self.running,
'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,
}
@@ -1140,6 +1326,11 @@ ros2_manager = fc_ros_manager()
3. 新增 _publish_position_ned() 發布剛體座標的訊息
4. 重製 _publish_attitude() 使其更簡單的去發布姿態訊息
2026.05.11
1. 新增 RtcmRelay node - 訂閱 mavros_msgs/RTCM topic 轉發為 MAVLink GPS_RTCM_DATA
2. 實作過期丟棄(1s)、blake2s hash 去重、最短轉發間隔節流、分片(180 bytes/片)
3. 接入 fc_ros_manager 的 initialize/start/stop/shutdown 生命週期
TODO
1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期
2. 還沒測試 如果有失效狀態讓 fc_ros_manager 中斷了 如何恢復的問題