From 5c14c458237782a613dd0d7829c0b7bb8d29ce42 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Wed, 13 May 2026 17:28:14 +0800 Subject: [PATCH] =?UTF-8?q?#=20RTK=20function=20update=20---=201.=20mavlin?= =?UTF-8?q?kROS2Node=20=E6=96=B0=E5=A2=9E=20RtcmRelay=20Node=20=E7=9B=A3?= =?UTF-8?q?=E8=81=BD=20RTCM=20topic=20=E4=B8=A6=E8=BD=89=E7=99=BC=E5=88=B0?= =?UTF-8?q?=20mavlinkObject=20bus=20(GPS=5FRTCM=5FDATA)=202.=20=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=20fc=5Fnetwork=5Fmodule=20ros2=20package=20=E4=BD=9C?= =?UTF-8?q?=E7=82=BA=E5=8D=94=E5=90=8C=E9=99=84=E5=B1=AC=E6=A8=A1=E7=B5=84?= =?UTF-8?q?=E7=9A=84=E6=94=BE=E7=BD=AE=E7=A9=BA=E9=96=93=203.=20Create=20a?= =?UTF-8?q?=20new=20fc=5Fnetwork=5Fmodule=20with=20NtripClientNode=20for?= =?UTF-8?q?=20connecting=20to=20NTRIP=20casters=20and=20publishing=20RTCM?= =?UTF-8?q?=20data=20to=20ROS2=20topics.=204.=20(modify)=20mainOrchestrato?= =?UTF-8?q?r=20adjust=20the=20publishing=20rates=20for=20GNSS=20topics?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 27 ++- .../fc_network_adapter/fc_network_adapter.md | 21 +- .../fc_network_adapter/mainOrchestrator.py | 6 +- .../fc_network_adapter/mavlinkROS2Nodes.py | 203 +++++++++++++++++- .../fc_network_module/__init__.py | 0 .../fc_network_module/ntrip_client.py | 146 +++++++++++++ src/fc_network_module/package.xml | 22 ++ .../resource/fc_network_module | 0 src/fc_network_module/setup.cfg | 4 + src/fc_network_module/setup.py | 26 +++ src/fc_network_module/test/test_copyright.py | 25 +++ src/fc_network_module/test/test_flake8.py | 25 +++ src/fc_network_module/test/test_pep257.py | 23 ++ src/unitdev02/unitdev02/devnote.txt | 14 +- 14 files changed, 519 insertions(+), 23 deletions(-) create mode 100644 src/fc_network_module/fc_network_module/__init__.py create mode 100644 src/fc_network_module/fc_network_module/ntrip_client.py create mode 100644 src/fc_network_module/package.xml create mode 100644 src/fc_network_module/resource/fc_network_module create mode 100644 src/fc_network_module/setup.cfg create mode 100644 src/fc_network_module/setup.py create mode 100644 src/fc_network_module/test/test_copyright.py create mode 100644 src/fc_network_module/test/test_flake8.py create mode 100644 src/fc_network_module/test/test_pep257.py diff --git a/README.md b/README.md index 86b77a2..717c75d 100644 --- a/README.md +++ b/README.md @@ -73,25 +73,30 @@ python gui.py === ## 資料夾說明 -1. unitdev 為各自協作者做開發時的測試區 - 01 -> 晉凱(ken) - 02 -> 其宇(chiyu) - 03 -> 文鈞 - 04 -> 倫渝 -2. fc_network_adapter (核心) +1. fc_network_adapter (核心) 建立、維護與飛控韌體的連接 構築 mavlink 封包 處理無線模組的通訊格式 (XBee) --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- -3. fc_interfaces (重要) - 自定義的 ROS2 介面檔 +2. fc_interfaces (重要) + 自定義的 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 是執行時期的記錄檔 === diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md index dc87053..b234bb6 100644 --- a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md +++ b/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 - 會開一個執行緒 讓兩個 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指令 diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 37c8b42..ddf2933 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/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_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, diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 5e03fc2..ff8a4f2 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/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 @@ -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 中斷了 如何恢復的問題 diff --git a/src/fc_network_module/fc_network_module/__init__.py b/src/fc_network_module/fc_network_module/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_module/fc_network_module/ntrip_client.py b/src/fc_network_module/fc_network_module/ntrip_client.py new file mode 100644 index 0000000..41ff58a --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/fc_network_module/package.xml b/src/fc_network_module/package.xml new file mode 100644 index 0000000..e82cd24 --- /dev/null +++ b/src/fc_network_module/package.xml @@ -0,0 +1,22 @@ + + + + fc_network_module + 0.0.0 + TODO: Package description + picars + TODO: License declaration + + rclpy + mavros_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/fc_network_module/resource/fc_network_module b/src/fc_network_module/resource/fc_network_module new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_module/setup.cfg b/src/fc_network_module/setup.cfg new file mode 100644 index 0000000..df65ffa --- /dev/null +++ b/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 diff --git a/src/fc_network_module/setup.py b/src/fc_network_module/setup.py new file mode 100644 index 0000000..035c668 --- /dev/null +++ b/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', + ], + }, +) diff --git a/src/fc_network_module/test/test_copyright.py b/src/fc_network_module/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/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' diff --git a/src/fc_network_module/test/test_flake8.py b/src/fc_network_module/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/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) diff --git a/src/fc_network_module/test/test_pep257.py b/src/fc_network_module/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/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' diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index d8309aa..835e70d 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/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