From 2c7f2afc450ca66aac74fcf66a48d7bfa104d702 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 8 Jun 2026 23:09:14 +0800 Subject: [PATCH] (Add) ntrip_client.py GGA sentence generation and sending --- .../fc_network_module/ntrip_client.py | 111 +++++++++++++++--- 1 file changed, 94 insertions(+), 17 deletions(-) diff --git a/src/fc_network_module/fc_network_module/ntrip_client.py b/src/fc_network_module/fc_network_module/ntrip_client.py index 41ff58a..04b63d5 100644 --- a/src/fc_network_module/fc_network_module/ntrip_client.py +++ b/src/fc_network_module/fc_network_module/ntrip_client.py @@ -8,11 +8,65 @@ from rclpy.node import Node from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy from mavros_msgs.msg import RTCM +# TODO: ROS_DOMAIN_ID 要補一下 + +class GGA_stream(): + + @classmethod + def nmea_checksum(cls, body: str) -> str: + """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'""" + value = 0 + for ch in body: + value ^= ord(ch) + return f"{value:02X}" + + @classmethod + def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> tuple[str, str]: + """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。""" + if is_latitude: + hemi = "N" if deg >= 0 else "S" + deg_width = 2 + else: + hemi = "E" if deg >= 0 else "W" + deg_width = 3 + deg = abs(deg) + d = int(deg) + m = (deg - d) * 60.0 + return f"{d:0{deg_width}d}{m:07.4f}", hemi + + @classmethod + def build_gga_sentence(cls, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes: + """ + 組一筆 $GPGGA 句子（含 checksum) 回傳 bytes 可直接 sock.sendall。 + + lat_deg / lon_deg : 十進位經緯度(北、東為正)。 + 測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。 + """ + utc = time.gmtime() + t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00" + + lat_dm, ns = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True) + lon_dm, ew = cls.decimal_to_nmea_dm(lon_deg, is_latitude=False) + + # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意 + body = ( + f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew}," + f"1,08,1.0,{alt_m:.1f},M,0.0,M,," + ) + sentence = f"${body}*{cls.nmea_checksum(body)}\r\n" + return sentence.encode("ascii") + + @classmethod + def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str: + """送出 GGA，回傳可讀句子供列印。""" + payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m) + sock.sendall(payload) + return payload.decode("ascii").strip() class NtripClientNode(Node): - """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。""" + """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """ - RECONNECT_BASE_SEC = 2.0 + RECONNECT_BASE_SEC = 10.0 RECONNECT_MAX_SEC = 60.0 def __init__(self): @@ -21,9 +75,15 @@ class NtripClientNode(Node): self.declare_parameter('host', 'rtk2go.com') self.declare_parameter('port', 2101) self.declare_parameter('mountpoint', '') - self.declare_parameter('username', '') + self.declare_parameter('username', 'template@mail.com') self.declare_parameter('password', '') + self.declare_parameter('gga_on', False) + self.declare_parameter('gga_lat', None) + self.declare_parameter('gga_lon', None) + self.declare_parameter('gga_alt_m', 100) + self.declare_parameter('gga_interval_sec', 300) + rtcm_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, depth=1, @@ -43,28 +103,36 @@ class NtripClientNode(Node): def _receive_loop(self): backoff = self.RECONNECT_BASE_SEC + host = self.get_parameter('host').value + port = self.get_parameter('port').value + mountpoint = self.get_parameter('mountpoint').value + user = self.get_parameter('username').value + pwd = self.get_parameter('password').value + + if not mountpoint: + self.get_logger().error('mountpoint 參數未設定...') + return + 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) + self.gga_lat = self.get_parameter('gga_lat').value + self.gga_lon = self.get_parameter('gga_lon').value + self.gga_alt_m = self.get_parameter('gga_alt_m').value + self.gga_interval_sec = self.get_parameter('gga_interval_sec').value + self.gga_on = self.get_parameter('gga_on').value + self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on + + try: - self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...') + self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...') sock.connect((host, port)) auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode() request = ( - f'GET /{mount} HTTP/1.0\r\n' + f'GET /{mountpoint} 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' @@ -78,7 +146,9 @@ class NtripClientNode(Node): ) raise ConnectionError('handshake failed') - self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流') + self.get_logger().info(f'已成功連接至 {mountpoint}') + + backoff = self.RECONNECT_BASE_SEC self._read_stream(sock) @@ -93,14 +163,21 @@ class NtripClientNode(Node): def _read_stream(self, sock: socket.socket): buf = b'' + last_gga_time = 0.0 while self._running: + now = time.time() + if self.gga_on and (now - last_gga_time >= self.gga_interval_sec): + line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m) + self.get_logger().info(f"[GGA] {line}") + last_gga_time = now + try: chunk = sock.recv(4096) except socket.timeout: continue if not chunk: - break + raise ConnectionError("stream ended. Empty Chunk") buf += chunk while len(buf) >= 6: