# RTK function update

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

README.md

@@ -73,25 +73,30 @@ python gui.py
 
 ===
 ## 資料夾說明
-1. unitdev 為各自協作者做開發時的測試區
+1. fc_network_adapter (核心)
-    01 -> 晉凱(ken)
-    02 -> 其宇(chiyu)
-    03 -> 文鈞
-    04 -> 倫渝
-2. fc_network_adapter (核心)
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
     --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
-3. fc_interfaces (重要)
+2. fc_interfaces (重要)
-    自定義的 ROS2 介面檔
+    自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了
+3. fc_network_module (重要)
+    非核心 但是支援載具的重要附屬功能 
+    需要該功能時 作為一個 ros2 節點打開
+    例如 : ntrip rtk 訊號轉接
 4. fc_network_apps
-    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用
+    與 fc_network_adapter 銜接做高階功能包裝的應用小程式 
-    使用者的外層包裝
+    利於開發GUI或其他應用 使用者的外層包裝
+    這裡的定位是 "核心功能的高階包裝" 可以完全不去用
 5. someotherpkg
     如何使用 fc_network_apps 的範例檔案
 6. GUI 
     由 PyQt6 開發的互動式介面
-N. logs 是執行時期的記錄檔
+7. unitdev 為各自協作者做開發時的測試區
+    01 -> 晉凱(ken)
+    02 -> 其宇(chiyu)
+    03 -> 文鈞
+    04 -> 倫渝
+8. logs 是執行時期的記錄檔
 
 ===

src/fc_interfaces/CMakeLists.txt

@@ -9,13 +9,15 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/AttitudeRaw.msg"
+  "msg/GnssRaw.msg"
   "srv/MavPing.srv"
   "srv/MavCommandLong.srv"
   "srv/MavPositionTargetGlobalInt.srv"
-  DEPENDENCIES std_msgs
+  DEPENDENCIES std_msgs builtin_interfaces
   )
 
 ament_package()

src/fc_interfaces/msg/GnssRaw.msg

@@ -0,0 +1,8 @@
+builtin_interfaces/Time stamp
+float64 latitude
+float64 longitude
+float64 altitude
+uint8 fix_type
+uint8 satellites_visible
+float32 eph
+float32 epv

src/fc_interfaces/package.xml

@@ -17,6 +17,8 @@
 
   <build_depend>std_msgs</build_depend>
   <exec_depend>std_msgs</exec_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md

@@ -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
+    MAVLink ROS2 節點管理器 管理三個獨立的 Node
-    會開一個執行緒 讓兩個 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

@@ -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': 1.0,
+            'position_gnss': 0.2,
-            'position_ned': 1.0,
+            'position_ned': 0.5,
-            'attitude': 1.0,
+            'attitude': 0.1,
             'velocity': 0.0,
             'battery': 1.0,
             'vfr_hud': 1.0,

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -109,6 +109,7 @@ class mavlink_bridge:
         """初始化訊息處理器映射表，提高處理效率"""
         self.message_handlers = {
             0: self._handle_heartbeat,        # HEARTBEAT
+            24: self._handle_gps_raw_int,     # GPS_RAW_INT
             30: self._handle_attitude,        # ATTITUDE
             32: self._handle_local_position,  # LOCAL_POSITION_NED
             33: self._handle_global_position, # GLOBAL_POSITION_INT
@@ -239,6 +240,14 @@ class mavlink_bridge:
         component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
         component.status.position.timestamp = timestamp
 
+    def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
+        """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
+        component.status.gps.fix_type = msg.fix_type
+        component.status.gps.satellites_visible = msg.satellites_visible
+        component.status.gps.eph = None if msg.eph == 65535 else msg.eph
+        component.status.gps.epv = None if msg.epv == 65535 else msg.epv
+        component.status.gps.timestamp = timestamp
+
     def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
         """處理 VFR_HUD 訊息 (msg_id: 74)"""
         component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +395,9 @@ class mavlink_object:
             self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
         
         # 記錄訊息過濾類型 (可選)
-        self.bridge_msg_types = set([0, 30, 32, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED 
+        # 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, 
+        # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
+        self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147])  
         self.return_msg_types = set([])
 
         # 轉發到別的 mavlink object 作為目標端口 的列表

src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py

@@ -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
@@ -61,7 +65,7 @@ class PublishRateController:
         # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
         # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
         self.topic_intervals = {
-            'position': 0.0,       # GNSS位置
+            'position_gnss': 0.0,  # GNSS位置
             'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
             'attitude': 0.0,       # 姿態 (pitch yaw row 與其加速狀態)
             'velocity': 0.0,       # 速度 (已經包含在 vfr_hud 未來移除)
@@ -131,8 +135,8 @@ class VehicleStatusPublisher(Node):
         self.fc_publishers: Dict[tuple, any] = {}
         
         # 定時器：以較高頻率檢查 vehicle_registry 並發布
-        # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
+        # 20Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
-        self.timer_period = 0.1  # 100ms
+        self.timer_period = 0.05  # 50ms
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
         
         # 狀態標誌
@@ -171,7 +175,7 @@ class VehicleStatusPublisher(Node):
         # 例如：self._publish_ekf_status(sysid, status)
         # ═══════════════════════════════════════════════════════════════
         # 發布各種狀態（通過頻率控制器判斷是否發布）
-        self._publish_position(sysid, status)
+        self._publish_position_gnss(sysid, status)
         self._publish_position_ned(sysid, status)
         self._publish_attitude(sysid, status)
         self._publish_velocity(sysid, status)
@@ -202,9 +206,9 @@ class VehicleStatusPublisher(Node):
             logger.info(f"Created publisher: {topic_name}")
         return self.fc_publishers[key]
     
-    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
+    def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
         """發布 GPS 位置"""
-        if not self.rate_controller.should_publish(sysid, 'position'):
+        if not self.rate_controller.should_publish(sysid, 'position_gnss'):
             return
         
         pos = status.position
@@ -212,22 +216,27 @@ class VehicleStatusPublisher(Node):
             return
         
         publisher = self._get_or_create_publisher(
-            sysid, 'position', sensor_msgs.msg.NavSatFix
+            sysid, 'position_gnss', fcmsg.GnssRaw
             )
         
         # 檢查是否有訂閱者
         if publisher.get_subscription_count() == 0:
             return
         
-        msg = sensor_msgs.msg.NavSatFix()
+        msg = fcmsg.GnssRaw()
-        msg.latitude = pos.latitude
+        msg.stamp = self.get_clock().now().to_msg()
-        msg.longitude = pos.longitude
+        msg.latitude = float(pos.latitude)
-        msg.altitude = pos.altitude if pos.altitude is not None else 0.0
+        msg.longitude = float(pos.longitude)
+        msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
         
         # GPS 狀態資訊
         gps = status.gps
         if gps.fix_type is not None:
-            msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
+            msg.fix_type = int(gps.fix_type)
+        if gps.satellites_visible is not None:
+            msg.satellites_visible = int(gps.satellites_visible)
+        msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
+        msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
         
         publisher.publish(msg)
 
@@ -431,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,
         }
@@ -950,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 節點管理器
 # ============================================================================
@@ -963,9 +1145,10 @@ class fc_ros_manager:
     stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。
     shutdown 是完全關閉 ROS2 並銷毀節點實例。
 
-    管理兩個獨立的 ROS2 Node：
+    管理三個獨立的 ROS2 Node：
     - VehicleStatusPublisher
     - MavlinkCommandService
+    - RtcmRelay
     
     提供統一的啟動/停止介面給 mainOrchestrator
     """
@@ -974,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
@@ -996,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")
@@ -1064,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():
@@ -1088,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():
@@ -1105,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,
         }
 
 
@@ -1135,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 中斷了 如何恢復的問題

src/fc_network_adapter/tests/test_vehicleStatusPublisher.py

@@ -17,6 +17,7 @@ import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
 import mavros_msgs.msg
+import fc_interfaces.msg
 
 # 專案 imports
 from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
         
         self.sysid = sysid
         self.received_messages = {
-            'position': [],
+            'position_gnss': [],
             'attitude': [],
             'velocity': [],
             'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
         
         # Position
         self.create_subscription(
-            sensor_msgs.msg.NavSatFix,
+            fc_interfaces.msg.GnssRaw,
-            f'{base_topic}/position',
+            f'{base_topic}/position_gnss',
-            lambda msg: self._on_message('position', msg),
+            lambda msg: self._on_message('position_gnss', msg),
             10
         )
         
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
     
     def _format_msg(self, topic_name: str, msg) -> str:
         """格式化消息以便顯示"""
-        if topic_name == 'position':
+        if topic_name == 'position_gnss':
             return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
         elif topic_name == 'attitude':
             return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
     # 檢查收到的消息
     print("\n--- 消息統計 ---")
     total_messages = 0
-    for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
         count = test_sub.get_message_count(topic)
         total_messages += count
         print(f"  {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
     # 修改頻率設定
     publisher_node = ros2_manager.status_publisher
     publisher_node.rate_controller.topic_intervals = {
-        'position': 1.5,   
+        'position_gnss': 1.5,   
         'attitude': 1.0,    
         'velocity': 1.0,     
         'battery': 1.0,   
@@ -329,7 +330,7 @@ def test_frequency_control():
     print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
     
     print("2Hz Topics (預期 ~6 條):")
-    for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+    for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
         count = test_sub.get_message_count(topic)
         print(f"  {topic:15s}: {count:3d} 條")
     for topic in ['battery', 'mode', 'summary']:

src/fc_network_module/fc_network_module/ntrip_client.py

@@ -0,0 +1,146 @@
+import socket
+import base64
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy
+from mavros_msgs.msg import RTCM
+
+
+class NtripClientNode(Node):
+    """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。"""
+
+    RECONNECT_BASE_SEC = 2.0
+    RECONNECT_MAX_SEC = 60.0
+
+    def __init__(self):
+        super().__init__('ntrip_client')
+
+        self.declare_parameter('host', 'rtk2go.com')
+        self.declare_parameter('port', 2101)
+        self.declare_parameter('mountpoint', '')
+        self.declare_parameter('username', '')
+        self.declare_parameter('password', '')
+
+        rtcm_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=1,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+        )
+        self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos)
+
+        self._running = True
+        self._thread = threading.Thread(target=self._receive_loop, daemon=True)
+        self._thread.start()
+
+        self.get_logger().info('NtripClientNode 已啟動')
+
+    # ── NTRIP 連線 + RTCM 接收迴圈 ──────────────────────────────
+
+    def _receive_loop(self):
+        backoff = self.RECONNECT_BASE_SEC
+
+        while self._running:
+            host = self.get_parameter('host').value
+            port = self.get_parameter('port').value
+            mount = self.get_parameter('mountpoint').value
+            user = self.get_parameter('username').value
+            pwd = self.get_parameter('password').value
+
+            if not mount:
+                self.get_logger().error('mountpoint 參數未設定，等待重試...')
+                time.sleep(backoff)
+                continue
+
+            sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            sock.settimeout(10)
+
+            try:
+                self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...')
+                sock.connect((host, port))
+
+                auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode()
+                request = (
+                    f'GET /{mount} HTTP/1.0\r\n'
+                    f'User-Agent: NTRIP PythonClient\r\n'
+                    f'Authorization: Basic {auth}\r\n'
+                    f'Connection: close\r\n\r\n'
+                )
+                sock.sendall(request.encode())
+
+                resp = sock.recv(4096)
+                if b'ICY 200 OK' not in resp:
+                    self.get_logger().error(
+                        f'NTRIP 握手失敗: {resp.decode(errors="ignore").strip()}'
+                    )
+                    raise ConnectionError('handshake failed')
+
+                self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流')
+                backoff = self.RECONNECT_BASE_SEC
+
+                self._read_stream(sock)
+
+            except Exception as e:
+                if self._running:
+                    self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連')
+                    time.sleep(backoff)
+                    backoff = min(backoff * 2, self.RECONNECT_MAX_SEC)
+            finally:
+                sock.close()
+
+    def _read_stream(self, sock: socket.socket):
+        buf = b''
+
+        while self._running:
+            try:
+                chunk = sock.recv(4096)
+            except socket.timeout:
+                continue
+            if not chunk:
+                break
+            buf += chunk
+
+            while len(buf) >= 6:
+                if buf[0] != 0xD3:
+                    buf = buf[1:]
+                    continue
+
+                payload_len = ((buf[1] & 0x03) << 8) | buf[2]
+                frame_len = payload_len + 6  # header(3) + payload + CRC(3)
+
+                if len(buf) < frame_len:
+                    break
+
+                packet = buf[:frame_len]
+                buf = buf[frame_len:]
+
+                self._publish_rtcm(packet)
+
+    def _publish_rtcm(self, raw_packet: bytes):
+        msg = RTCM()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.data = list(raw_packet)
+        self.publisher_.publish(msg)
+
+    def destroy_node(self):
+        self._running = False
+        self._thread.join(timeout=3.0)
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NtripClientNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/fc_network_module/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fc_network_module</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiyu1468@hotmail.com">picars</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>mavros_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/fc_network_module/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/fc_network_module
+[install]
+install_scripts=$base/lib/fc_network_module

src/fc_network_module/setup.py

@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'fc_network_module'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='picars',
+    maintainer_email='chiyu1468@hotmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'ntrip_client = fc_network_module.ntrip_client:main',
+        ],
+    },
+)

src/fc_network_module/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/fc_network_module/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/fc_network_module/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/unitdev02/unitdev02/devnote.txt

@@ -7,9 +7,17 @@
  - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) 
 
 這一步
- 研究 ros2 service
+ 希望 ackEnum 可以擺到正確的位置
+ node 增加 GNSS 訊息 (2d fix 3d fix rtk float...)
+ RTK
+   跟 RTK2go 抓取列表
+   從特定 mount point 得到數據
+   做一個 ros2 service(action?) 接到數據並包裝
+ 
 
 下一步
+  NODE 重啟
+  可以 refresh node topic
 
 下下一步
  
@@ -18,6 +26,8 @@
  rssi 資訊提取s
 
 
+mavlink timeout - CommLONG 這個是 mavlink bus 沒有回應 可能是丟包了
+
 
 自己的常用指令
 python -m fc_network_adapter.tests.test_vehicleStatusPublisher
@@ -27,6 +37,8 @@ python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
 python -m someotherpkg.src.example_takeoff_land
 python -m someotherpkg.src.example_change_mode
 
+python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpoint:=No1bio_02 -p username:=chiyu1468@hotmail.com
+
 ros2 topic list
 ros2 topic echo /fc_network/vehicle/sys3/battery