init

CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.5)
+project(nmea_ros_bridge)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_executable(nmea_tcp src/nmea_tcp.cpp )
+ament_auto_add_executable(nmea_udp src/nmea_udp.cpp)
+ament_auto_add_executable(nmea_serial src/nmea_serial.cpp)
+ament_auto_add_executable(serial_to_tcp_server src/serial_to_tcp_server.cpp)
+ament_auto_add_executable(ntrip_to_tcp_server src/ntrip_to_tcp_server.cpp)
+ament_auto_add_executable(serial_ntrip_nmea src/serial_ntrip_nmea.cpp)
+
+
+install(TARGETS 
+  nmea_tcp
+  nmea_udp
+  nmea_serial
+  serial_to_tcp_server
+  ntrip_to_tcp_server
+  serial_ntrip_nmea
+  DESTINATION lib/$(PROJECT_NAME)
+)
+
+install(DIRECTORY config launch
+  DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

LICENSE

@@ -0,0 +1,45 @@
+Copyright 2021 Map IV, Inc. All rights reserved.
+
+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.
+
+---
+
+BSD 3-Clause License
+
+Copyright (c) 2021, MeijoMeguroLab
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,32 @@
+# nmea_ros_bridge
+
+## Overview
+nmea_ros_bridge converts the NMEA of a GNSS receiver to a ROS topic.
+
+Requires [nmea_msgs](http://wiki.ros.org/nmea_msgs).
+
+## Install
+
+1) First, download the ros2ized nmea_msgs and nmea_ros_bridge to your colcon_ws directory.
+
+        cd $HOME/colcon_ws/src
+        sudo apt-get install ros-foxy-nmea-msgs
+        git clone -b ros2-v0.1.0 https://github.com/MapIV/nmea_ros_bridge.git
+
+2) Build nmea_ros_bridge
+
+        cd $HOME/colcon_ws/
+        colcon build
+
+## Usage
+1) Connect the GNSS receiver and start nmea_ros_bridge(tcp).
+
+        cd $HOME/colcon_ws/
+        source install/setup.bash
+        ros2 launch nmea_ros_bridge nmea_tcp.launch.py
+
+2) Connect the GNSS receiver and start nmea_ros_bridge(udp).
+
+        cd $HOME/colcon_ws/
+        source install/setup.bash
+        ros2 launch nmea_ros_bridge nmea_udp.launch.py

config/ntrip_to_tcp_server_config.yaml

@@ -0,0 +1,12 @@
+ntrip_to_tcp_server:
+  ros__parameters:
+    serial_port: "/dev/ttyUSB0"
+    baud_rate: 115200
+    ntrip_host: "120.253.226.97"  # 服务IP地址（备用IP: 120.253.239.161）
+    ntrip_port: 8001              # 服务端口
+    mountpoint: "RTCM33_GRCE"     # 接入点
+    username: "cwtm11609"          # CORS账号
+    password: "9hemate7"           # CORS密码
+    tcp_host: "0.0.0.0"
+    tcp_port: 62001
+    nmea_tcp_port: 62002  # 新增NMEA数据端口

config/serial_config.yaml

@@ -0,0 +1,7 @@
+nmea_serial:
+  ros__parameters:
+    device: "/dev/ttyUSB0"                 # 串口设备路径
+    baud_rate: 115200                      # 波特率 (9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600)
+    nmea_topic: navsat/nmea_sentence       # 修改为新的NMEA话题名称
+    frame_id: sentence                     # NMEA话题的frame_id
+    rate: 0.0                              # 接收缓冲区监控频率 (0.0表示阻塞模式)

config/serial_ntrip_nmea_config.yaml

@@ -0,0 +1,11 @@
+serial_ntrip_nmea:
+  ros__parameters:
+    serial_port: "/dev/ttyUSB0"
+    baud_rate: 115200
+    ntrip_host: "120.253.226.97"  # 服务IP地址（备用IP: 120.253.239.161）
+    ntrip_port: 8001              # 服务端口
+    mountpoint: "RTCM33_GRCE"     # 接入点
+    username: "cwtm11609"          # CORS账号
+    password: "9hemate7"           # CORS密码
+    nmea_topic: "navsat/nmea_sentence"
+    frame_id: "sentence"

config/serial_to_tcp_server_config.yaml

@@ -0,0 +1,6 @@
+serial_to_tcp_server:
+  ros__parameters:
+    serial_port: "/dev/ttyUSB0"
+    baud_rate: 115200
+    tcp_host: "0.0.0.0"
+    tcp_port: 62002

config/tcp_config.yaml

@@ -0,0 +1,7 @@
+nmea_tcp:
+  ros__parameters:
+    address: "0.0.0.0"              # Destination IPAdress()
+    port: 62002                            # Destination Port()
+    nmea_topic: navsat/nmea_sentence              # Publish NMEA topic name
+    frame_id: sentence                     # NMEA topic frame_id
+    rate: 0.0                              # watch receive buffer rate

config/udp_config.yaml

@@ -0,0 +1,7 @@
+nmea_udp:
+  ros__parameters:
+    address: "192.168.30.10"              # Destination IPAdress()
+    port: 62002                           # Destination Port()
+    nmea_topic: nmea_sentence              # Publish NMEA topic name
+    frame_id: sentence                     # NMEA topic frame_id
+    rate: 0.0                              # watch receive buffer rat

launch/nmea_serial.launch.py

@@ -0,0 +1,25 @@
+import os
+
+import ament_index_python.packages
+import launch
+import launch_ros.actions
+import yaml
+
+def generate_launch_description():
+  share_dir = ament_index_python.packages.get_package_share_directory('nmea_ros_bridge')
+  params_file = os.path.join(share_dir, 'config', 'serial_config.yaml')
+  with open(params_file, 'r') as f:
+    params = yaml.safe_load(f)['nmea_serial']['ros__parameters']
+  nmea_serial = launch_ros.actions.Node(package='nmea_ros_bridge',
+                                        executable='nmea_serial',
+                                        output='screen',
+                                        parameters=[params])
+  
+  return launch.LaunchDescription([nmea_serial,
+                                  launch.actions.RegisterEventHandler(
+                                      event_handler=launch.event_handlers.OnProcessExit(
+                                          target_action=nmea_serial,
+                                          on_exit=[launch.actions.EmitEvent(
+                                              event=launch.events.Shutdown())],
+                                      )),
+                                  ])

launch/nmea_tcp.launch.py

@@ -0,0 +1,26 @@
+
+import os
+
+import ament_index_python.packages
+import launch
+import launch_ros.actions
+import yaml
+
+def generate_launch_description():
+  share_dir = ament_index_python.packages.get_package_share_directory('nmea_ros_bridge')
+  params_file = os.path.join(share_dir, 'config', 'tcp_config.yaml')
+  with open(params_file, 'r') as f:
+    params = yaml.safe_load(f)['nmea_tcp']['ros__parameters']
+  nmea_tcp = launch_ros.actions.Node(package='nmea_ros_bridge',
+                                      executable='nmea_tcp',
+                                      output='screen',
+                                      parameters=[params])
+  
+  return launch.LaunchDescription([nmea_tcp,
+                                  launch.actions.RegisterEventHandler(
+                                      event_handler=launch.event_handlers.OnProcessExit(
+                                          target_action=nmea_tcp,
+                                          on_exit=[launch.actions.EmitEvent(
+                                              event=launch.events.Shutdown())],
+                                      )),
+                                  ])

launch/nmea_udp.launch.py

@@ -0,0 +1,27 @@
+
+import os
+
+import ament_index_python.packages
+import launch
+import launch_ros.actions
+import yaml
+
+def generate_launch_description():
+  share_dir = ament_index_python.packages.get_package_share_directory('nmea_ros_bridge')
+  params_file = os.path.join(share_dir, 'config', 'udp_config.yaml')
+  with open(params_file, 'r') as f:
+    params = yaml.safe_load(f)['nmea_udp']['ros__parameters']
+  nmea_udp = launch_ros.actions.Node(package='nmea_ros_bridge',
+                                      executable='nmea_udp',
+                                      namespace='navsat',
+                                      output='screen',
+                                      parameters=[params])
+
+  return launch.LaunchDescription([nmea_udp,
+                                  launch.actions.RegisterEventHandler(
+                                      event_handler=launch.event_handlers.OnProcessExit(
+                                          target_action=nmea_udp,
+                                          on_exit=[launch.actions.EmitEvent(
+                                              event=launch.events.Shutdown())],
+                                      )),
+                                  ])

launch/ntrip_to_tcp_server.launch.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+"""
+C++版本NTRIP到TCP服务器启动文件（配置文件版本）
+使用YAML配置文件启动C++版本的NTRIP到TCP服务器
+"""
+
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # 获取包共享目录和配置文件路径
+    share_dir = get_package_share_directory('nmea_ros_bridge')
+    config_file = os.path.join(share_dir, 'config', 'ntrip_to_tcp_server_config.yaml')
+    
+    # 读取配置文件
+    with open(config_file, 'r') as f:
+        params = yaml.safe_load(f)['ntrip_to_tcp_server']['ros__parameters']
+    
+    # C++版本NTRIP到TCP服务器节点
+    ntrip_to_tcp_server_node = Node(
+        package='nmea_ros_bridge',
+        executable='ntrip_to_tcp_server',
+        name='ntrip_to_tcp_server',
+        output='screen',
+        parameters=[params]
+    )
+
+    return LaunchDescription([
+        ntrip_to_tcp_server_node
+    ])

launch/serial_ntrip_nmea.launch.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""
+serial_ntrip_nmea节点的启动文件
+"""
+
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch.actions
+import launch.event_handlers
+
+def generate_launch_description():
+    # 获取包共享目录和配置文件路径
+    share_dir = get_package_share_directory('nmea_ros_bridge')
+    config_file = os.path.join(share_dir, 'config', 'serial_ntrip_nmea_config.yaml')
+    
+    # 读取配置文件
+    with open(config_file, 'r') as f:
+        params = yaml.safe_load(f)['serial_ntrip_nmea']['ros__parameters']
+    
+    # serial_ntrip_nmea节点
+    serial_ntrip_nmea_node = Node(
+        package='nmea_ros_bridge',
+        executable='serial_ntrip_nmea',
+        name='serial_ntrip_nmea',
+        output='screen',
+        parameters=[params]
+    )
+
+    return LaunchDescription([
+        serial_ntrip_nmea_node,
+        launch.actions.RegisterEventHandler(
+            event_handler=launch.event_handlers.OnProcessExit(
+                target_action=serial_ntrip_nmea_node,
+                on_exit=[launch.actions.EmitEvent(
+                    event=launch.events.Shutdown())],
+            )),
+    ])

launch/serial_to_tcp_server.launch.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python3
+"""
+C++版本串口到TCP服务器启动文件（配置文件版本）
+使用YAML配置文件启动C++版本的串口到TCP服务器
+"""
+
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # 获取包共享目录和配置文件路径
+    share_dir = get_package_share_directory('nmea_ros_bridge')
+    config_file = os.path.join(share_dir, 'config', 'serial_to_tcp_server_config.yaml')
+    
+    # 读取配置文件
+    with open(config_file, 'r') as f:
+        params = yaml.safe_load(f)['serial_to_tcp_server']['ros__parameters']
+    
+    # C++版本串口到TCP服务器节点
+    serial_to_tcp_server_node = Node(
+        package='nmea_ros_bridge',
+        executable='serial_to_tcp_server',
+        name='serial_to_tcp_server',
+        output='screen',
+        parameters=[params]
+    )
+
+    return LaunchDescription([
+        serial_to_tcp_server_node
+    ])

package.xml

@@ -0,0 +1,23 @@
+<?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>nmea_ros_bridge</name>
+  <version>0.0.0</version>
+  <description>The nmea_ros_bridge package</description>
+  <maintainer email="megken@todo.todo">megken</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>nmea_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/nmea_serial.cpp

@@ -0,0 +1,226 @@
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <cstring>  // 添加strerror头文件
+#include <cerrno>   // 添加errno头文件
+
+#include "rclcpp/rclcpp.hpp"
+#include "nmea_msgs/msg/sentence.hpp"
+
+#define BUFFER_SAFE 2000
+
+#include "rclcpp/time.hpp"
+#include <chrono>
+#include "rclcpp/clock.hpp"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/logger.hpp"
+
+rclcpp::Node::SharedPtr node = nullptr;
+rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr pub;
+
+// 配置串口参数
+static bool configure_serial_port(int fd, int baud_rate)
+{
+    struct termios tty;
+    
+    if (tcgetattr(fd, &tty) != 0) {
+        RCLCPP_ERROR(node->get_logger(), "Error getting terminal attributes: %s", strerror(errno));
+        return false;
+    }
+
+    // 设置波特率
+    speed_t speed;
+    switch (baud_rate) {
+        case 9600: speed = B9600; break;
+        case 19200: speed = B19200; break;
+        case 38400: speed = B38400; break;
+        case 57600: speed = B57600; break;
+        case 115200: speed = B115200; break;
+        case 230400: speed = B230400; break;
+        case 460800: speed = B460800; break;
+        case 921600: speed = B921600; break;
+        default:
+            RCLCPP_ERROR(node->get_logger(), "Unsupported baud rate: %d", baud_rate);
+            return false;
+    }
+    
+    cfsetospeed(&tty, speed);
+    cfsetispeed(&tty, speed);
+
+    // 设置串口参数
+    tty.c_cflag &= ~PARENB; // 无奇偶校验
+    tty.c_cflag &= ~CSTOPB; // 1位停止位
+    tty.c_cflag &= ~CSIZE;
+    tty.c_cflag |= CS8;     // 8位数据位
+    tty.c_cflag &= ~CRTSCTS; // 无硬件流控
+    tty.c_cflag |= (CREAD | CLOCAL); // 启用接收，忽略调制解调器控制线
+
+    // 输入模式设置 - 禁用所有输入处理
+    tty.c_lflag = 0;        // 禁用所有本地模式标志
+    tty.c_iflag = 0;        // 禁用所有输入处理
+    tty.c_oflag = 0;        // 禁用所有输出处理
+
+    // 控制字符 - 设置非阻塞读取
+    tty.c_cc[VTIME] = 0;    // 读取超时（十分之一秒）
+    tty.c_cc[VMIN] = 0;     // 读取最小字符数（0表示非阻塞）
+
+    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
+        RCLCPP_ERROR(node->get_logger(), "Error setting terminal attributes: %s", strerror(errno));
+        return false;
+    }
+
+    // 清空串口缓冲区
+    tcflush(fd, TCIOFLUSH);
+    
+    return true;
+}
+
+static void packet_receive(int fd, std::string frame_id, double rate)
+{
+    nmea_msgs::msg::Sentence nmea_msg;
+    nmea_msg.header.frame_id = frame_id;
+    
+    rclcpp::Rate loop_rate(rate > 0.0 ? rate : 10.0); // 默认10Hz
+    
+    char buffer[4096];  // 增大缓冲区
+    char* read_ptr = buffer;
+    char* buffer_end = buffer + sizeof(buffer) - 1; // 保留一个字节给null终止符
+    
+    while (rclcpp::ok())
+    {
+        // 清空缓冲区
+        memset(buffer, 0, sizeof(buffer));
+        read_ptr = buffer;
+        
+        // 读取串口数据
+        ssize_t bytes_read = read(fd, buffer, sizeof(buffer) - 1);
+        
+        if (bytes_read > 0) {
+            buffer[bytes_read] = '\0'; // 确保字符串终止
+            
+            RCLCPP_DEBUG(node->get_logger(), "Read %zd bytes from serial", bytes_read);
+            
+            // 解析NMEA语句
+            char* data_ptr = buffer;
+            while (data_ptr < buffer + bytes_read) {
+                // 查找NMEA语句开始符 '$'
+                char* sentence_start = strchr(data_ptr, '$');
+                if (sentence_start == nullptr) {
+                    break; // 没有找到更多NMEA语句
+                }
+                
+                // 查找NMEA语句结束符 '\r' 或 '\n'
+                char* sentence_end = strpbrk(sentence_start, "\r\n");
+                if (sentence_end == nullptr) {
+                    // 没有完整的语句，等待更多数据
+                    break;
+                }
+                
+                // 提取完整的NMEA语句
+                size_t sentence_length = sentence_end - sentence_start;
+                if (sentence_length > 0) {
+                    std::string nmea_sentence(sentence_start, sentence_length);
+                    
+                    // 发布NMEA语句
+                    nmea_msg.header.stamp = node->now();
+                    nmea_msg.sentence = nmea_sentence;
+                    pub->publish(nmea_msg);
+                    
+                    RCLCPP_DEBUG(node->get_logger(), "Published NMEA: %s", nmea_sentence.c_str());
+                }
+                
+                // 移动到下一个可能的语句
+                data_ptr = sentence_end + 1;
+            }
+        }
+        else if (bytes_read == 0) {
+            // 没有数据可读，正常情况
+            RCLCPP_DEBUG(node->get_logger(), "No data available");
+        }
+        else {
+            // 读取错误
+            if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                RCLCPP_ERROR(node->get_logger(), "Error reading from serial port: %s", strerror(errno));
+                break;
+            }
+        }
+        
+        rclcpp::spin_some(node);
+        
+        if (rate > 0.0) {
+            loop_rate.sleep();
+        } else {
+            // 如果没有设置速率限制，使用小的延迟避免CPU过度使用
+            usleep(10000); // 10ms延迟
+        }
+    }
+    
+    RCLCPP_INFO(node->get_logger(), "Serial reader exiting");
+}
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    node = rclcpp::Node::make_shared("nmea_serial");
+    
+    // 读取参数
+    std::string device = "/dev/ttyUSB0";
+    int baud_rate = 115200;
+    std::string nmea_topic = "navsat/nmea_sentence";  // 修改默认值为新话题
+    std::string frame_id = "sentence";
+    double rate = 0.0;
+
+    node->declare_parameter("device", "/dev/ttyUSB0");
+    node->declare_parameter("baud_rate", 115200);
+    node->declare_parameter("nmea_topic", "navsat/nmea_sentence");  // 修改默认值为新话题
+    node->declare_parameter("frame_id", "sentence");
+    node->declare_parameter("rate", 0.0);
+
+    device = node->get_parameter("device").as_string();
+    baud_rate = node->get_parameter("baud_rate").as_int();
+    nmea_topic = node->get_parameter("nmea_topic").as_string();
+    frame_id = node->get_parameter("frame_id").as_string();
+    rate = node->get_parameter("rate").as_double();
+
+    pub = node->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic, 10);
+
+    RCLCPP_INFO(node->get_logger(), "Device: %s", device.c_str());
+    RCLCPP_INFO(node->get_logger(), "Baud Rate: %d", baud_rate);
+    RCLCPP_INFO(node->get_logger(), "NMEA Topic: %s", nmea_topic.c_str());
+    RCLCPP_INFO(node->get_logger(), "Frame ID: %s", frame_id.c_str());
+    RCLCPP_INFO(node->get_logger(), "Rate: %f", rate);
+
+    // 打开串口设备 - 使用非阻塞模式
+    int serial_fd = open(device.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+    if (serial_fd < 0) {
+        RCLCPP_ERROR(node->get_logger(), "Error opening serial device %s: %s", 
+                     device.c_str(), strerror(errno));
+        return -1;
+    }
+
+    // 配置串口参数
+    if (!configure_serial_port(serial_fd, baud_rate)) {
+        close(serial_fd);
+        return -1;
+    }
+
+    RCLCPP_INFO(node->get_logger(), "Serial port %s configured successfully", device.c_str());
+
+    try {
+        // 运行串口读取循环
+        packet_receive(serial_fd, frame_id, rate);
+    }
+    catch (const std::exception& e) {
+        RCLCPP_ERROR(node->get_logger(), "Exception in packet_receive: %s", e.what());
+    }
+
+    // 清理资源
+    close(serial_fd);
+    rclcpp::shutdown();
+    
+    RCLCPP_INFO(node->get_logger(), "nmea_serial node exited cleanly");
+    return 0;
+}

src/nmea_tcp.cpp

@@ -0,0 +1,323 @@
+/*
+ * Copyright 2021 Map IV, Inc. All rights reserved.
+ *
+ * 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.
+ */
+
+// BSD 3-Clause License
+
+// Copyright (c) 2021, MeijoMeguroLab
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// 1. Redistributions of source code must retain the above copyright notice, this
+//    list of conditions and the following disclaimer.
+
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+
+// 3. Neither the name of the copyright holder nor the names of its
+//    contributors may be used to endorse or promote products derived from
+//    this software without specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+/*
+ * nmea_tcp.cpp
+ *
+ * Author Kashimoto
+ * Ver 1.00 2021/03/19
+ */
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nmea_msgs/msg/sentence.hpp"
+
+#define BUFFER_SAFE 2000
+
+#include "rclcpp/time.hpp"
+#include <chrono>
+#include "rclcpp/clock.hpp"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/logger.hpp"
+
+rclcpp::Node::SharedPtr node = nullptr;
+rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr pub;
+
+struct sockaddr_in dstAddr;
+static bool isConnected = false;
+
+static void packet_receive_rate(int fd, std::string frame_id, double rate)
+{
+  nmea_msgs::msg::Sentence nmea_msg;
+  nmea_msg.header.frame_id = frame_id.c_str();
+  rclcpp::Rate loop_rate(rate);
+
+  int rem;
+  int ret;
+  char buffer[2048];
+  char* w_buffer = buffer;
+  char* buffer_end = &buffer[sizeof(buffer)];
+
+  while (rclcpp::ok())
+  {
+    errno = 0;
+    ret = read(fd, w_buffer, buffer_end - w_buffer - 1);
+    if (ret > 0)
+    {
+      if (strnlen(w_buffer, ret) != ret)
+      {
+        w_buffer = buffer;
+        continue;
+      }
+      w_buffer += ret;
+    }
+    else if (ret == 0)
+    {
+      rclcpp::shutdown();
+      close(fd);
+      return;
+    }
+    else
+    {
+      if (errno == EAGAIN)
+      {
+        continue;
+      }
+      else
+      {
+        rclcpp::shutdown();
+      }
+    }
+
+    *w_buffer = '\0';
+    char* r_buffer = buffer;
+
+    rclcpp::Clock ros_clock(RCL_ROS_TIME);
+    while (1)
+    {
+      char* sentence = strchr(r_buffer, '$');
+      if (sentence == NULL)
+      {
+        break;
+      }
+      char* end_sentence = strchr(sentence, '\r');
+      if (end_sentence == NULL)
+      {
+        break;
+      }
+      *end_sentence = '\0';
+
+      nmea_msg.header.stamp = ros_clock.now();
+      nmea_msg.sentence = sentence;
+      pub->publish(nmea_msg);
+      r_buffer = end_sentence + 1;
+    }
+
+    rem = w_buffer - r_buffer;
+    if (rem > BUFFER_SAFE)
+    {
+      RCLCPP_WARN(node->get_logger(), "Buffer over.Init Buffer.");
+      rem = 0;
+    }
+    memmove(buffer, r_buffer, rem);
+    w_buffer = buffer + rem;
+
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+  close(fd);
+}
+
+static void packet_receive_no_rate(int fd, std::string frame_id)
+{
+  nmea_msgs::msg::Sentence nmea_msg;
+  nmea_msg.header.frame_id = frame_id.c_str();
+
+
+  int rem;
+  int ret;
+  char buffer[2048];
+  char* w_buffer = buffer;
+  char* buffer_end = &buffer[sizeof(buffer)];
+
+  while (rclcpp::ok())
+  {
+    errno = 0;
+    ret = read(fd, w_buffer, buffer_end - w_buffer - 1);
+    if (ret > 0)
+    {
+      if (strnlen(w_buffer, ret) != ret)
+      {
+        w_buffer = buffer;
+        continue;
+      }
+      w_buffer += ret;
+    }
+    else if (ret == 0)
+    {
+      rclcpp::shutdown();
+      close(fd);
+      return;
+    }
+    else
+    {
+      if (errno == EAGAIN)
+      {
+        continue;
+      }
+      else
+      {
+        rclcpp::shutdown();
+      }
+    }
+
+    *w_buffer = '\0';
+    char* r_buffer = buffer;
+
+    rclcpp::Clock ros_clock(RCL_ROS_TIME);
+    while (1)
+    {
+      char* sentence = strchr(r_buffer, '$');
+      if (sentence == NULL)
+      {
+        break;
+      }
+      char* end_sentence = strchr(sentence, '\r');
+      if (end_sentence == NULL)
+      {
+        break;
+      }
+      *end_sentence = '\0';
+
+      nmea_msg.header.stamp = ros_clock.now();
+      nmea_msg.sentence = sentence;
+      // std::cout << "nmea_msg.sentence " << nmea_msg.sentence <<std::endl;
+      pub->publish(nmea_msg);
+      r_buffer = end_sentence + 1;
+    }
+
+    rem = w_buffer - r_buffer;
+    if (rem > BUFFER_SAFE)
+    {
+      RCLCPP_WARN(node->get_logger(), "Buffer over.Init Buffer.");
+      rem = 0;
+    }
+    memmove(buffer, r_buffer, rem);
+    w_buffer = buffer + rem;
+
+    rclcpp::spin_some(node);
+
+  }
+  close(fd);
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  node = rclcpp::Node::make_shared("nmea_tcp");
+  pub = node->create_publisher<nmea_msgs::msg::Sentence>("nmea_topic", 10);
+
+  int sock;
+  int result,val;
+
+  // Read parameters
+  std::string address = "192.168.53.181";
+  int port = 62002;
+  std::string nmea_topic = "navsat/nmea_sentence";
+  std::string frame_id = "sentence";
+  double rate = 0.0;
+
+  node->declare_parameter("address", "192.168.53.181");
+  node->declare_parameter("port", 62002);
+  node->declare_parameter("nmea_topic", "navsat/nmea_sentence");
+  node->declare_parameter("frame_id", "sentence");
+  node->declare_parameter("rate", 0.0);
+
+  address = node->get_parameter("address").as_string();
+  port = node->get_parameter("port").as_int();
+  nmea_topic = node->get_parameter("nmea_topic").as_string();
+  frame_id = node->get_parameter("frame_id").as_string();
+  rate = node->get_parameter("rate").as_double();
+
+  pub = node->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic, 10);
+
+  RCLCPP_INFO(node->get_logger(), "IP: %s", address.c_str());
+  RCLCPP_INFO(node->get_logger(), "PORT: %d", port);
+  RCLCPP_INFO(node->get_logger(), "NMEA_TOPIC: %s", nmea_topic.c_str());
+  RCLCPP_INFO(node->get_logger(), "FRAME_ID: %s", frame_id.c_str());
+  RCLCPP_INFO(node->get_logger(), "RATE: %f", rate);
+
+  sock = socket(AF_INET, SOCK_STREAM, 0);
+
+  memset(&dstAddr, 0, sizeof(dstAddr));
+  dstAddr.sin_family = AF_INET;
+  dstAddr.sin_addr.s_addr = inet_addr(address.c_str());
+  dstAddr.sin_port = htons(port);
+
+  rclcpp::Rate loop_rate(1.0);
+  while (rclcpp::ok())
+  {
+    result = connect(sock, (struct sockaddr *) &dstAddr, sizeof(dstAddr));
+    if( result < 0 )
+    {
+      /* non-connect */
+      RCLCPP_INFO(node->get_logger(), "CONNECT TRY... ");
+    }
+    else
+    {
+      /* connect */
+      isConnected = true;
+      break;
+    }
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+
+  if ((isConnected == true)&&(rclcpp::ok()))
+  {
+    if( rate != 0.0 )
+    {
+      /* non-block *//* ros::rate() */
+      val = 1;
+      ioctl(sock, FIONBIO, &val);
+      packet_receive_rate(sock, frame_id, rate);
+    }
+    else
+    {
+      /* block */
+      packet_receive_no_rate(sock, frame_id);
+    }
+
+  }
+
+  return 0;
+}

src/nmea_udp.cpp

@@ -0,0 +1,323 @@
+/*
+ * Copyright 2021 Map IV, Inc. All rights reserved.
+ *
+ * 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.
+ */
+
+// BSD 3-Clause License
+
+// Copyright (c) 2021, MeijoMeguroLab
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// 1. Redistributions of source code must retain the above copyright notice, this
+//    list of conditions and the following disclaimer.
+
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+
+// 3. Neither the name of the copyright holder nor the names of its
+//    contributors may be used to endorse or promote products derived from
+//    this software without specific prior written permission.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+/*
+ * nmea_udp.cpp
+ *
+ * Author Kashimoto
+ * Ver 1.00 2021/03/19
+ */
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nmea_msgs/msg/sentence.hpp"
+
+#define BUFFER_SAFE 2000
+
+#include "rclcpp/time.hpp"
+#include <chrono>
+#include "rclcpp/clock.hpp"
+
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/logger.hpp"
+
+rclcpp::Node::SharedPtr node = nullptr;
+rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr pub;
+
+struct sockaddr_in dstAddr;
+static bool isConnected = false;
+
+static void packet_receive_rate(int fd, std::string frame_id, double rate)
+{
+  nmea_msgs::msg::Sentence nmea_msg;
+  nmea_msg.header.frame_id = frame_id.c_str();
+  rclcpp::Rate loop_rate(rate);
+
+  int rem;
+  int ret;
+  char buffer[2048];
+  char* w_buffer = buffer;
+  char* buffer_end = &buffer[sizeof(buffer)];
+
+  while (rclcpp::ok())
+  {
+    errno = 0;
+    ret = read(fd, w_buffer, buffer_end - w_buffer - 1);
+    if (ret > 0)
+    {
+      if (strnlen(w_buffer, ret) != ret)
+      {
+        w_buffer = buffer;
+        continue;
+      }
+      w_buffer += ret;
+    }
+    else if (ret == 0)
+    {
+      rclcpp::shutdown();
+      close(fd);
+      return;
+    }
+    else
+    {
+      if (errno == EAGAIN)
+      {
+        continue;
+      }
+      else
+      {
+        rclcpp::shutdown();
+      }
+    }
+
+    *w_buffer = '\0';
+    char* r_buffer = buffer;
+
+    rclcpp::Clock ros_clock(RCL_ROS_TIME);
+    while (1)
+    {
+      char* sentence = strchr(r_buffer, '$');
+      if (sentence == NULL)
+      {
+        break;
+      }
+      char* end_sentence = strchr(sentence, '\r');
+      if (end_sentence == NULL)
+      {
+        break;
+      }
+      *end_sentence = '\0';
+
+      nmea_msg.header.stamp = ros_clock.now();
+      nmea_msg.sentence = sentence;
+      pub->publish(nmea_msg);
+      r_buffer = end_sentence + 1;
+    }
+
+    rem = w_buffer - r_buffer;
+    if (rem > BUFFER_SAFE)
+    {
+      RCLCPP_WARN(node->get_logger(), "Buffer over.Init Buffer.");
+      rem = 0;
+    }
+    memmove(buffer, r_buffer, rem);
+    w_buffer = buffer + rem;
+
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+  close(fd);
+}
+
+static void packet_receive_no_rate(int fd, std::string frame_id)
+{
+  nmea_msgs::msg::Sentence nmea_msg;
+  nmea_msg.header.frame_id = frame_id.c_str();
+
+
+  int rem;
+  int ret;
+  char buffer[2048];
+  char* w_buffer = buffer;
+  char* buffer_end = &buffer[sizeof(buffer)];
+
+  while (rclcpp::ok())
+  {
+    errno = 0;
+    ret = read(fd, w_buffer, buffer_end - w_buffer - 1);
+    if (ret > 0)
+    {
+      if (strnlen(w_buffer, ret) != ret)
+      {
+        w_buffer = buffer;
+        continue;
+      }
+      w_buffer += ret;
+    }
+    else if (ret == 0)
+    {
+      rclcpp::shutdown();
+      close(fd);
+      return;
+    }
+    else
+    {
+      if (errno == EAGAIN)
+      {
+        continue;
+      }
+      else
+      {
+        rclcpp::shutdown();
+      }
+    }
+
+    *w_buffer = '\0';
+    char* r_buffer = buffer;
+
+    rclcpp::Clock ros_clock(RCL_ROS_TIME);
+    while (1)
+    {
+      char* sentence = strchr(r_buffer, '$');
+      if (sentence == NULL)
+      {
+        break;
+      }
+      char* end_sentence = strchr(sentence, '\r');
+      if (end_sentence == NULL)
+      {
+        break;
+      }
+      *end_sentence = '\0';
+
+      nmea_msg.header.stamp = ros_clock.now();
+      nmea_msg.sentence = sentence;
+      // std::cout << "nmea_msg.sentence " << nmea_msg.sentence <<std::endl;
+      pub->publish(nmea_msg);
+      r_buffer = end_sentence + 1;
+    }
+
+    rem = w_buffer - r_buffer;
+    if (rem > BUFFER_SAFE)
+    {
+      RCLCPP_WARN(node->get_logger(), "Buffer over.Init Buffer.");
+      rem = 0;
+    }
+    memmove(buffer, r_buffer, rem);
+    w_buffer = buffer + rem;
+
+    rclcpp::spin_some(node);
+
+  }
+  close(fd);
+}
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  node = rclcpp::Node::make_shared("nmea_udp");
+
+  int sock;
+  int result,val;
+
+  // Read parameters
+  std::string address = "192.168.30.10";
+  int port = 62002;
+  std::string nmea_topic = "navsat/nmea_sentence";
+  std::string frame_id = "sentence";
+  double rate = 0.0;
+
+  node->declare_parameter("address","192.168.30.10");
+  node->declare_parameter("port", 62002);
+  node->declare_parameter("nmea_topic", "navsat/nmea_sentence");
+  node->declare_parameter("frame_id", "sentence");
+  node->declare_parameter("rate", 0.0);
+
+  address = node->get_parameter("address").as_string();
+  port = node->get_parameter("port").as_int();
+  nmea_topic = node->get_parameter("nmea_topic").as_string();
+  frame_id = node->get_parameter("frame_id").as_string();
+  rate = node->get_parameter("rate").as_double();
+
+  pub = node->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic, 10);
+
+  RCLCPP_INFO(node->get_logger(), "IP: %s", address.c_str());
+  RCLCPP_INFO(node->get_logger(), "PORT: %d", port);
+  RCLCPP_INFO(node->get_logger(), "NMEA_TOPIC: %s", nmea_topic.c_str());
+  RCLCPP_INFO(node->get_logger(), "FRAME_ID: %s", frame_id.c_str());
+  RCLCPP_INFO(node->get_logger(), "RATE: %f", rate);
+
+  sock = socket(AF_INET, SOCK_DGRAM, 0);
+
+  memset(&dstAddr, 0, sizeof(dstAddr));
+  dstAddr.sin_family = AF_INET;
+  dstAddr.sin_addr.s_addr = INADDR_ANY;
+  dstAddr.sin_port = htons(port);
+
+  rclcpp::Rate loop_rate(1.0);
+  while (rclcpp::ok())
+  {
+    //result = connect(sock, (struct sockaddr *) &dstAddr, sizeof(dstAddr));
+    result = bind(sock, (struct sockaddr *)&dstAddr, sizeof(dstAddr));
+    if( result < 0 )
+    {
+      /* non-connect */
+      RCLCPP_INFO(node->get_logger(), "CONNECT TRY... ");
+    }
+    else
+    {
+      /* connect */
+      isConnected = true;
+      break;
+    }
+    rclcpp::spin_some(node);
+    loop_rate.sleep();
+  }
+
+  if ((isConnected == true)&&(rclcpp::ok()))
+  {
+    if( rate != 0.0 )
+    {
+      /* non-block *//* rclcpp::rate() */
+      val = 1;
+      ioctl(sock, FIONBIO, &val);
+      packet_receive_rate(sock, frame_id, rate);
+    }
+    else
+    {
+      /* block */
+      packet_receive_no_rate(sock, frame_id);
+    }
+
+  }
+
+  return 0;
+}

src/ntrip_to_tcp_server.cpp

@@ -0,0 +1,723 @@
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <cstring>
+#include <cerrno>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <sstream>
+#include <iomanip>
+#include <signal.h>
+
+#include "rclcpp/rclcpp.hpp"
+
+class NtripToTCPServer : public rclcpp::Node
+{
+public:
+    NtripToTCPServer() : Node("ntrip_to_tcp_server")
+    {
+        // 声明参数
+        this->declare_parameter("serial_port", "/dev/ttyUSB0");
+        this->declare_parameter("baud_rate", 115200);
+        this->declare_parameter("ntrip_host", "103.143.19.54");
+        this->declare_parameter("ntrip_port", 8002);
+        this->declare_parameter("mountpoint", "RTCM32GRCpro");
+        this->declare_parameter("username", "7ripg398");
+        this->declare_parameter("password", "97344");
+        this->declare_parameter("tcp_host", "0.0.0.0");
+        this->declare_parameter("tcp_port", 62001);
+        this->declare_parameter("nmea_tcp_port", 62002);  // 新增NMEA TCP端口参数
+        
+        // 获取参数
+        serial_port_ = this->get_parameter("serial_port").as_string();
+        baud_rate_ = this->get_parameter("baud_rate").as_int();
+        ntrip_host_ = this->get_parameter("ntrip_host").as_string();
+        ntrip_port_ = this->get_parameter("ntrip_port").as_int();
+        mountpoint_ = this->get_parameter("mountpoint").as_string();
+        username_ = this->get_parameter("username").as_string();
+        password_ = this->get_parameter("password").as_string();
+        tcp_host_ = this->get_parameter("tcp_host").as_string();
+        tcp_port_ = this->get_parameter("tcp_port").as_int();
+        nmea_tcp_port_ = this->get_parameter("nmea_tcp_port").as_int();  // 获取NMEA端口参数
+        
+        RCLCPP_INFO(this->get_logger(), "NtripToTCPServer初始化:");
+        RCLCPP_INFO(this->get_logger(), "  串口: %s @ %d baud", serial_port_.c_str(), baud_rate_);
+        RCLCPP_INFO(this->get_logger(), "  NTRIP服务器: %s:%d", ntrip_host_.c_str(), ntrip_port_);
+        RCLCPP_INFO(this->get_logger(), "  挂载点: %s", mountpoint_.c_str());
+        RCLCPP_INFO(this->get_logger(), "  RTCM TCP服务器: %s:%d", tcp_host_.c_str(), tcp_port_);
+        RCLCPP_INFO(this->get_logger(), "  NMEA TCP服务器: %s:%d", tcp_host_.c_str(), nmea_tcp_port_);
+        
+        // 启动服务器
+        if (setup_serial() && setup_tcp_servers() && connect_ntrip()) {
+            running_ = true;
+            start_server();
+        }
+    }
+    
+    ~NtripToTCPServer()
+    {
+        running_ = false;
+        if (serial_fd_ >= 0) close(serial_fd_);
+        if (rtcm_tcp_socket_ >= 0) close(rtcm_tcp_socket_);
+        if (nmea_tcp_socket_ >= 0) close(nmea_tcp_socket_);
+        if (ntrip_socket_ >= 0) close(ntrip_socket_);
+        for (auto& client : rtcm_client_sockets_) {
+            if (client >= 0) close(client);
+        }
+        for (auto& client : nmea_client_sockets_) {
+            if (client >= 0) close(client);
+        }
+    }
+
+private:
+    std::string serial_port_;
+    int baud_rate_;
+    std::string ntrip_host_;
+    int ntrip_port_;
+    std::string mountpoint_;
+    std::string username_;
+    std::string password_;
+    std::string tcp_host_;
+    int tcp_port_;
+    int nmea_tcp_port_;  // 新增NMEA端口变量
+    
+    int serial_fd_{-1};
+    int rtcm_tcp_socket_{-1};  // 重命名为RTCM TCP socket
+    int nmea_tcp_socket_{-1};  // 新增NMEA TCP socket
+    int ntrip_socket_{-1};
+    std::vector<int> rtcm_client_sockets_;  // 重命名为RTCM客户端
+    std::vector<int> nmea_client_sockets_;  // 新增NMEA客户端
+    std::atomic<bool> running_{false};
+    std::atomic<bool> ntrip_connected_{false};
+    std::mutex rtcm_clients_mutex_;  // 重命名为RTCM客户端锁
+    std::mutex nmea_clients_mutex_;  // 新增NMEA客户端锁
+    
+    bool setup_serial()
+    {
+        serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+        if (serial_fd_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "串口打开失败: %s - %s", 
+                         serial_port_.c_str(), strerror(errno));
+            return false;
+        }
+        
+        struct termios tty;
+        if (tcgetattr(serial_fd_, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "获取串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 设置波特率
+        speed_t speed;
+        switch (baud_rate_) {
+            case 9600: speed = B9600; break;
+            case 19200: speed = B19200; break;
+            case 38400: speed = B38400; break;
+            case 57600: speed = B57600; break;
+            case 115200: speed = B115200; break;
+            case 230400: speed = B230400; break;
+            default:
+                RCLCPP_ERROR(this->get_logger(), "不支持的波特率: %d", baud_rate_);
+                return false;
+        }
+        
+        cfsetospeed(&tty, speed);
+        cfsetispeed(&tty, speed);
+        
+        // 设置串口参数
+        tty.c_cflag &= ~PARENB;
+        tty.c_cflag &= ~CSTOPB;
+        tty.c_cflag &= ~CSIZE;
+        tty.c_cflag |= CS8;
+        tty.c_cflag &= ~CRTSCTS;
+        tty.c_cflag |= (CREAD | CLOCAL);
+        
+        tty.c_lflag = 0;
+        tty.c_iflag = 0;
+        tty.c_oflag = 0;
+        
+        tty.c_cc[VTIME] = 0;
+        tty.c_cc[VMIN] = 0;
+        
+        if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "设置串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        tcflush(serial_fd_, TCIOFLUSH);
+        RCLCPP_INFO(this->get_logger(), "串口连接成功: %s", serial_port_.c_str());
+        return true;
+    }
+    
+    bool setup_tcp_servers()  // 修改函数名
+    {
+        // 设置RTCM TCP服务器
+        rtcm_tcp_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (rtcm_tcp_socket_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "创建RTCM TCP socket失败: %s", strerror(errno));
+            return false;
+        }
+        
+        int opt = 1;
+        setsockopt(rtcm_tcp_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+        
+        struct sockaddr_in rtcm_server_addr;
+        rtcm_server_addr.sin_family = AF_INET;
+        rtcm_server_addr.sin_addr.s_addr = inet_addr(tcp_host_.c_str());
+        rtcm_server_addr.sin_port = htons(tcp_port_);
+        
+        if (bind(rtcm_tcp_socket_, (struct sockaddr*)&rtcm_server_addr, sizeof(rtcm_server_addr)) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "RTCM TCP绑定失败: %s", strerror(errno));
+            return false;
+        }
+        
+        if (listen(rtcm_tcp_socket_, 5) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "RTCM TCP监听失败: %s", strerror(errno));
+            return false;
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "RTCM TCP服务器启动成功: %s:%d", tcp_host_.c_str(), tcp_port_);
+        
+        // 设置NMEA TCP服务器
+        nmea_tcp_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (nmea_tcp_socket_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "创建NMEA TCP socket失败: %s", strerror(errno));
+            return false;
+        }
+        
+        setsockopt(nmea_tcp_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+        
+        struct sockaddr_in nmea_server_addr;
+        nmea_server_addr.sin_family = AF_INET;
+        nmea_server_addr.sin_addr.s_addr = inet_addr(tcp_host_.c_str());
+        nmea_server_addr.sin_port = htons(nmea_tcp_port_);
+        
+        if (bind(nmea_tcp_socket_, (struct sockaddr*)&nmea_server_addr, sizeof(nmea_server_addr)) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "NMEA TCP绑定失败: %s", strerror(errno));
+            return false;
+        }
+        
+        if (listen(nmea_tcp_socket_, 5) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "NMEA TCP监听失败: %s", strerror(errno));
+            return false;
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "NMEA TCP服务器启动成功: %s:%d", tcp_host_.c_str(), nmea_tcp_port_);
+        
+        return true;
+    }
+    
+    bool connect_ntrip()
+    {
+        ntrip_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (ntrip_socket_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "创建NTRIP socket失败: %s", strerror(errno));
+            return false;
+        }
+        
+        struct sockaddr_in ntrip_addr;
+        ntrip_addr.sin_family = AF_INET;
+        ntrip_addr.sin_port = htons(ntrip_port_);
+        ntrip_addr.sin_addr.s_addr = inet_addr(ntrip_host_.c_str());
+        
+        if (connect(ntrip_socket_, (struct sockaddr*)&ntrip_addr, sizeof(ntrip_addr)) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "连接NTRIP服务器失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 构建NTRIP请求
+        std::string auth_string = username_ + ":" + password_;
+        std::string auth_b64 = base64_encode(auth_string);
+        
+        std::stringstream request;
+        request << "GET /" << mountpoint_ << " HTTP/1.1\r\n";
+        request << "User-Agent: NTRIP NtripToTCPServer/1.0\r\n";
+        request << "Authorization: Basic " << auth_b64 << "\r\n";
+        request << "Accept: gnss/data\r\n";
+        request << "Connection: close\r\n";
+        request << "\r\n";
+        
+        std::string request_str = request.str();
+        
+        // 打印发送给NTRIP的请求内容
+        RCLCPP_INFO(this->get_logger(), "发送给NTRIP服务器的请求:");
+        RCLCPP_INFO(this->get_logger(), "========================================");
+        RCLCPP_INFO(this->get_logger(), "%s", request_str.c_str());
+        RCLCPP_INFO(this->get_logger(), "========================================");
+        
+        ssize_t sent = send(ntrip_socket_, request_str.c_str(), request_str.length(), 0);
+        if (sent < 0) {
+            RCLCPP_ERROR(this->get_logger(), "发送NTRIP请求失败: %s", strerror(errno));
+            return false;
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "成功发送NTRIP请求: %zd bytes", sent);
+        
+        // 读取响应
+        char buffer[1024];
+        ssize_t received = recv(ntrip_socket_, buffer, sizeof(buffer) - 1, 0);
+        if (received > 0) {
+            buffer[received] = '\0';
+            std::string response(buffer);
+            
+            // 打印从NTRIP接收到的响应内容
+            RCLCPP_INFO(this->get_logger(), "从NTRIP服务器接收到的响应:");
+            RCLCPP_INFO(this->get_logger(), "========================================");
+            RCLCPP_INFO(this->get_logger(), "%s", response.c_str());
+            RCLCPP_INFO(this->get_logger(), "========================================");
+            RCLCPP_INFO(this->get_logger(), "响应大小: %zd bytes", received);
+            
+            if (response.find("200") != std::string::npos) {
+                ntrip_connected_ = true;
+                RCLCPP_INFO(this->get_logger(), "NTRIP连接成功: %s", mountpoint_.c_str());
+                return true;
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "NTRIP连接失败: %s", response.c_str());
+            }
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "未收到NTRIP服务器响应");
+        }
+        
+        return false;
+    }
+    
+    void start_server()
+    {
+        // 启动客户端接受线程
+        std::thread accept_thread(&NtripToTCPServer::accept_rtcm_clients, this);
+        accept_thread.detach();
+        
+        // 启动NMEA客户端接受线程
+        std::thread nmea_accept_thread(&NtripToTCPServer::accept_nmea_clients, this);
+        nmea_accept_thread.detach();
+        
+        // 启动NTRIP数据转发线程
+        std::thread ntrip_thread(&NtripToTCPServer::read_ntrip_and_forward, this);
+        ntrip_thread.detach();
+        
+        // 启动串口数据读取和NTRIP发送线程
+        std::thread serial_thread(&NtripToTCPServer::read_serial_and_send_to_ntrip, this);
+        serial_thread.detach();
+        
+        // 主循环 - 显示状态信息
+        rclcpp::Rate rate(1); // 1Hz
+        while (rclcpp::ok() && running_) {
+            {
+                std::lock_guard<std::mutex> rtcm_lock(rtcm_clients_mutex_);
+                std::lock_guard<std::mutex> nmea_lock(nmea_clients_mutex_);
+                RCLCPP_INFO(this->get_logger(), "运行状态 - RTCM客户端: %zu, NMEA客户端: %zu, NTRIP连接: %s", 
+                           rtcm_client_sockets_.size(), nmea_client_sockets_.size(), ntrip_connected_ ? "是" : "否");
+            }
+            rate.sleep();
+        }
+    }
+    
+    void accept_rtcm_clients()
+    {
+        while (rclcpp::ok() && running_) {
+            struct sockaddr_in client_addr;
+            socklen_t client_len = sizeof(client_addr);
+            
+            int client_socket = accept(rtcm_tcp_socket_, (struct sockaddr*)&client_addr, &client_len);
+            if (client_socket >= 0) {
+                int flags = fcntl(client_socket, F_GETFL, 0);
+                fcntl(client_socket, F_SETFL, flags | O_NONBLOCK);
+                
+                {
+                    std::lock_guard<std::mutex> lock(rtcm_clients_mutex_);
+                    rtcm_client_sockets_.push_back(client_socket);
+                }
+                
+                RCLCPP_INFO(this->get_logger(), "RTCM客户端连接: %s:%d", 
+                           inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+                
+                std::thread client_thread(&NtripToTCPServer::handle_rtcm_client, this, client_socket, client_addr);
+                client_thread.detach();
+            } else {
+                if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                    RCLCPP_ERROR(this->get_logger(), "接受RTCM客户端连接错误: %s", strerror(errno));
+                }
+                usleep(100000);
+            }
+        }
+    }
+    
+    void accept_nmea_clients()
+    {
+        while (rclcpp::ok() && running_) {
+            struct sockaddr_in client_addr;
+            socklen_t client_len = sizeof(client_addr);
+            
+            int client_socket = accept(nmea_tcp_socket_, (struct sockaddr*)&client_addr, &client_len);
+            if (client_socket >= 0) {
+                int flags = fcntl(client_socket, F_GETFL, 0);
+                fcntl(client_socket, F_SETFL, flags | O_NONBLOCK);
+                
+                {
+                    std::lock_guard<std::mutex> lock(nmea_clients_mutex_);
+                    nmea_client_sockets_.push_back(client_socket);
+                }
+                
+                RCLCPP_INFO(this->get_logger(), "NMEA客户端连接: %s:%d", 
+                           inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+                
+                std::thread client_thread(&NtripToTCPServer::handle_nmea_client, this, client_socket, client_addr);
+                client_thread.detach();
+            } else {
+                if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                    RCLCPP_ERROR(this->get_logger(), "接受NMEA客户端连接错误: %s", strerror(errno));
+                }
+                usleep(100000);
+            }
+        }
+    }
+    
+    void handle_rtcm_client(int client_socket, struct sockaddr_in client_addr)
+    {
+        while (rclcpp::ok() && running_) {
+            char test_buffer[1];
+            ssize_t result = send(client_socket, test_buffer, 0, MSG_DONTWAIT);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                break;
+            }
+            usleep(100000);
+        }
+        
+        close(client_socket);
+        {
+            std::lock_guard<std::mutex> lock(rtcm_clients_mutex_);
+            auto it = std::find(rtcm_client_sockets_.begin(), rtcm_client_sockets_.end(), client_socket);
+            if (it != rtcm_client_sockets_.end()) {
+                rtcm_client_sockets_.erase(it);
+            }
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "RTCM客户端断开: %s:%d", 
+                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+    }
+    
+    void handle_nmea_client(int client_socket, struct sockaddr_in client_addr)
+    {
+        while (rclcpp::ok() && running_) {
+            char test_buffer[1];
+            ssize_t result = send(client_socket, test_buffer, 0, MSG_DONTWAIT);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                break;
+            }
+            usleep(100000);
+        }
+        
+        close(client_socket);
+        {
+            std::lock_guard<std::mutex> lock(nmea_clients_mutex_);
+            auto it = std::find(nmea_client_sockets_.begin(), nmea_client_sockets_.end(), client_socket);
+            if (it != nmea_client_sockets_.end()) {
+                nmea_client_sockets_.erase(it);
+            }
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "NMEA客户端断开: %s:%d", 
+                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+    }
+    
+    void read_ntrip_and_forward()
+    {
+        std::vector<char> buffer(4096);
+        
+        while (rclcpp::ok() && running_ && ntrip_connected_) {
+            ssize_t bytes_read = recv(ntrip_socket_, buffer.data(), buffer.size(), MSG_DONTWAIT);
+            if (bytes_read > 0) {
+                // 打印接收到的RTCM数据信息
+                RCLCPP_INFO(this->get_logger(), "从NTRIP接收到RTCM数据: %zd bytes", bytes_read);
+                
+                // 显示前16字节的十六进制内容（用于调试）
+                if (bytes_read >= 16) {
+                    std::stringstream hex_dump;
+                    hex_dump << "RTCM数据前16字节: ";
+                    for (int i = 0; i < 16 && i < bytes_read; ++i) {
+                        hex_dump << std::hex << std::setw(2) << std::setfill('0') 
+                                << (static_cast<unsigned int>(buffer[i]) & 0xFF) << " ";
+                    }
+                    RCLCPP_DEBUG(this->get_logger(), "%s", hex_dump.str().c_str());
+                }
+                
+                // 转发RTCM数据给RTCM TCP客户端
+                forward_to_rtcm_clients(buffer.data(), bytes_read);
+                RCLCPP_INFO(this->get_logger(), "转发RTCM数据给 %zu 个RTCM客户端", rtcm_client_sockets_.size());
+                
+                // 同时将RTCM数据写入串口
+                ssize_t bytes_written = write(serial_fd_, buffer.data(), bytes_read);
+                if (bytes_written > 0) {
+                    RCLCPP_INFO(this->get_logger(), "写入串口RTCM数据: %zd bytes", bytes_written);
+                    
+                    // 显示写入串口的前16字节十六进制内容
+                    if (bytes_written >= 16) {
+                        std::stringstream hex_dump_serial;
+                        hex_dump_serial << "写入串口RTCM数据前16字节: ";
+                        for (int i = 0; i < 16 && i < bytes_written; ++i) {
+                            hex_dump_serial << std::hex << std::setw(2) << std::setfill('0') 
+                                          << (static_cast<unsigned int>(buffer[i]) & 0xFF) << " ";
+                        }
+                        RCLCPP_DEBUG(this->get_logger(), "%s", hex_dump_serial.str().c_str());
+                    }
+                } else if (bytes_written < 0) {
+                    if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                        RCLCPP_WARN(this->get_logger(), "串口写入缓冲区满，等待重试");
+                    } else {
+                        RCLCPP_ERROR(this->get_logger(), "串口写入错误: %s", strerror(errno));
+                    }
+                }
+                
+            } else if (bytes_read == 0) {
+                // NTRIP服务器断开连接
+                RCLCPP_ERROR(this->get_logger(), "NTRIP服务器断开连接");
+                ntrip_connected_ = false;
+                close(ntrip_socket_);
+                break;
+            } else {
+                // 修复：正确处理非阻塞读取的情况
+                if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                    // 这是正常情况，没有数据可读，等待一段时间后重试
+                    usleep(10000); // 10ms轮询间隔
+                } else {
+                    // 真正的错误情况
+                    RCLCPP_ERROR(this->get_logger(), "NTRIP读取错误: %s", strerror(errno));
+                    ntrip_connected_ = false;
+                    close(ntrip_socket_);
+                    break;
+                }
+            }
+            usleep(10000);
+        }
+        
+        // 尝试重新连接
+        if (running_ && !ntrip_connected_) {
+            RCLCPP_INFO(this->get_logger(), "尝试重新连接NTRIP服务器...");
+            usleep(5000000); // 5秒后重试
+            if (connect_ntrip()) {
+                std::thread ntrip_thread(&NtripToTCPServer::read_ntrip_and_forward, this);
+                ntrip_thread.detach();
+            }
+        }
+    }
+    
+    void forward_to_rtcm_clients(const char* data, size_t length)
+    {
+        std::lock_guard<std::mutex> lock(rtcm_clients_mutex_);
+        std::vector<int> disconnected_clients;
+        
+        for (int client_socket : rtcm_client_sockets_) {
+            ssize_t result = send(client_socket, data, length, MSG_DONTWAIT | MSG_NOSIGNAL);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                disconnected_clients.push_back(client_socket);
+            }
+        }
+        
+        for (int client : disconnected_clients) {
+            auto it = std::find(rtcm_client_sockets_.begin(), rtcm_client_sockets_.end(), client);
+            if (it != rtcm_client_sockets_.end()) {
+                close(client);
+                rtcm_client_sockets_.erase(it);
+            }
+        }
+    }
+    
+    void forward_to_nmea_clients(const char* data, size_t length)
+    {
+        std::lock_guard<std::mutex> lock(nmea_clients_mutex_);
+        std::vector<int> disconnected_clients;
+        
+        for (int client_socket : nmea_client_sockets_) {
+            ssize_t result = send(client_socket, data, length, MSG_DONTWAIT | MSG_NOSIGNAL);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                disconnected_clients.push_back(client_socket);
+            }
+        }
+        
+        for (int client : disconnected_clients) {
+            auto it = std::find(nmea_client_sockets_.begin(), nmea_client_sockets_.end(), client);
+            if (it != nmea_client_sockets_.end()) {
+                close(client);
+                nmea_client_sockets_.erase(it);
+            }
+        }
+    }
+    
+    void read_serial_and_send_to_ntrip()
+    {
+        std::vector<char> buffer(1024);
+        std::string nmea_buffer;
+        bool first_nmea_received = false;
+        
+        RCLCPP_INFO(this->get_logger(), "开始读取串口NMEA数据并发送到NTRIP服务器（仅发送GGA语句）");
+        
+        while (rclcpp::ok() && running_) {
+            if (!ntrip_connected_) {
+                RCLCPP_WARN(this->get_logger(), "NTRIP未连接，等待连接...");
+                usleep(1000000); // 1秒后重试
+                continue;
+            }
+            
+            ssize_t bytes_read = read(serial_fd_, buffer.data(), buffer.size());
+            if (bytes_read > 0) {
+                // 处理接收到的数据
+                for (ssize_t i = 0; i < bytes_read; ++i) {
+                    char c = buffer[i];
+                    nmea_buffer += c;
+                    
+                    // 检测NMEA语句结束符（换行符）
+                    if (c == '\n') {
+                        // 移除回车符（如果有）
+                        if (!nmea_buffer.empty() && nmea_buffer.back() == '\r') {
+                            nmea_buffer.pop_back();
+                        }
+                        
+                        // 检查是否为有效的NMEA语句（以$开头）
+                        if (!nmea_buffer.empty() && nmea_buffer[0] == '$') {
+                            // 检查是否为GGA语句（GPGGA或GNGGA）
+                            bool is_gga = false;
+                            if (nmea_buffer.length() >= 6) {
+                                std::string nmea_type = nmea_buffer.substr(1, 5); // 跳过$，取5个字符
+                                if (nmea_type == "GPGGA" || nmea_type == "GNGGA") {
+                                    is_gga = true;
+                                }
+                            }
+                            
+                            if (is_gga) {
+                                // 打印GGA语句
+                                RCLCPP_INFO(this->get_logger(), "接收到GGA语句: %s", nmea_buffer.c_str());
+                                
+                                if (!first_nmea_received) {
+                                    RCLCPP_INFO(this->get_logger(), "接收到第一个GGA语句: %s", nmea_buffer.c_str());
+                                    first_nmea_received = true;
+                                }
+                                
+                                // 发送GGA数据到NTRIP服务器
+                                ssize_t sent = send(ntrip_socket_, nmea_buffer.c_str(), nmea_buffer.length(), 
+                                                   MSG_DONTWAIT | MSG_NOSIGNAL);
+                                if (sent > 0) {
+                                    RCLCPP_DEBUG(this->get_logger(), "发送GGA数据到NTRIP: %s (%zd bytes)", 
+                                               nmea_buffer.c_str(), sent);
+                                } else if (sent < 0) {
+                                    if (errno == EPIPE || errno == ECONNRESET) {
+                                        RCLCPP_ERROR(this->get_logger(), "NTRIP连接断开，停止发送GGA数据");
+                                        ntrip_connected_ = false;
+                                        close(ntrip_socket_);  // 关闭套接字
+                                        break;
+                                    } else if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                                        RCLCPP_ERROR(this->get_logger(), "发送GGA数据错误: %s", strerror(errno));
+                                    }
+                                }
+                                
+                                // 定期显示发送的GGA数据（每10条显示一次）
+                                static int gga_count = 0;
+                                gga_count++;
+                                if (gga_count % 10 == 0) {
+                                    RCLCPP_INFO(this->get_logger(), "已发送 %d 条GGA语句到NTRIP服务器", gga_count);
+                                }
+                            } else {
+                                // 非GGA语句，记录但不发送到NTRIP
+                                // 但转发所有NMEA语句到NMEA TCP客户端
+                                RCLCPP_DEBUG(this->get_logger(), "接收到NMEA语句: %s", nmea_buffer.c_str());
+                            }
+                            
+                            // 转发所有NMEA语句到NMEA TCP客户端
+                            std::string nmea_with_newline = nmea_buffer + "\r\n";  // 添加回车换行符
+                            forward_to_nmea_clients(nmea_with_newline.c_str(), nmea_with_newline.length());
+                            RCLCPP_DEBUG(this->get_logger(), "转发NMEA语句给 %zu 个NMEA客户端", nmea_client_sockets_.size());
+                            
+                        } else if (!nmea_buffer.empty()) {
+                            //RCLCPP_WARN(this->get_logger(), "无效的NMEA语句格式: %s", nmea_buffer.c_str());
+                        }
+                        
+                        nmea_buffer.clear();
+                    }
+                }
+                
+                // 显示接收到的原始数据信息
+                RCLCPP_DEBUG(this->get_logger(), "从串口接收到 %zd bytes 数据", bytes_read);
+                
+            } else if (bytes_read == 0) {
+                // 修复：在非阻塞模式下，read()返回0表示没有数据可读，不是连接断开
+                // 这是正常情况，等待一段时间后重试
+                usleep(10000); // 10ms轮询间隔
+            } else {
+                // 修复：正确处理非阻塞读取的情况
+                if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                    // 这是正常情况，没有数据可读，等待一段时间后重试
+                    usleep(10000); // 10ms轮询间隔
+                } else {
+                    // 真正的错误情况
+                    RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
+                    break;
+                }
+            }
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "串口数据读取线程结束");
+    }
+    
+    std::string base64_encode(const std::string& input)
+    {
+        static const std::string base64_chars = 
+            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+            "abcdefghijklmnopqrstuvwxyz"
+            "0123456789+/";
+            
+        std::string encoded;
+        int i = 0;
+        int j = 0;
+        unsigned char char_array_3[3];
+        unsigned char char_array_4[4];
+        
+        for (char c : input) {
+            char_array_3[i++] = c;
+            if (i == 3) {
+                char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+                char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+                char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+                char_array_4[3] = char_array_3[2] & 0x3f;
+                
+                for(i = 0; i < 4; i++)
+                    encoded += base64_chars[char_array_4[i]];
+                i = 0;
+            }
+        }
+        
+        if (i) {
+            for(j = i; j < 3; j++)
+                char_array_3[j] = '\0';
+            
+            char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+            char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+            char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+            char_array_4[3] = char_array_3[2] & 0x3f;
+            
+            for (j = 0; j < i + 1; j++)
+                encoded += base64_chars[char_array_4[j]];
+            
+            while(i++ < 3)
+                encoded += '=';
+        }
+        
+        return encoded;
+    }
+};
+
+int main(int argc, char** argv)
+{
+    // 忽略SIGPIPE信号，防止程序因SIGPIPE而崩溃
+    signal(SIGPIPE, SIG_IGN);
+    
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<NtripToTCPServer>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/serial_ntrip_nmea.cpp

@@ -0,0 +1,412 @@
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <cstring>
+#include <cerrno>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <sstream>
+#include <iomanip>
+#include <signal.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nmea_msgs/msg/sentence.hpp"
+
+class SerialNtripNmea : public rclcpp::Node
+{
+public:
+    SerialNtripNmea() : Node("serial_ntrip_nmea")
+    {
+        // 声明参数
+        this->declare_parameter("serial_port", "/dev/ttyUSB0");
+        this->declare_parameter("baud_rate", 115200);
+        this->declare_parameter("ntrip_host", "103.143.19.54");
+        this->declare_parameter("ntrip_port", 8002);
+        this->declare_parameter("mountpoint", "RTCM32GRCpro");
+        this->declare_parameter("username", "7ripg398");
+        this->declare_parameter("password", "97344");
+        this->declare_parameter("nmea_topic", "navsat/nmea_sentence");
+        this->declare_parameter("frame_id", "sentence");
+        
+        // 获取参数
+        serial_port_ = this->get_parameter("serial_port").as_string();
+        baud_rate_ = this->get_parameter("baud_rate").as_int();
+        ntrip_host_ = this->get_parameter("ntrip_host").as_string();
+        ntrip_port_ = this->get_parameter("ntrip_port").as_int();
+        mountpoint_ = this->get_parameter("mountpoint").as_string();
+        username_ = this->get_parameter("username").as_string();
+        password_ = this->get_parameter("password").as_string();
+        nmea_topic_ = this->get_parameter("nmea_topic").as_string();
+        frame_id_ = this->get_parameter("frame_id").as_string();
+        
+        // 创建NMEA消息发布器
+        nmea_pub_ = this->create_publisher<nmea_msgs::msg::Sentence>(nmea_topic_, 10);
+        
+        RCLCPP_INFO(this->get_logger(), "SerialNtripNmea初始化:");
+        RCLCPP_INFO(this->get_logger(), "  串口: %s @ %d baud", serial_port_.c_str(), baud_rate_);
+        RCLCPP_INFO(this->get_logger(), "  NTRIP服务器: %s:%d", ntrip_host_.c_str(), ntrip_port_);
+        RCLCPP_INFO(this->get_logger(), "  挂载点: %s", mountpoint_.c_str());
+        RCLCPP_INFO(this->get_logger(), "  NMEA话题: %s", nmea_topic_.c_str());
+        
+        // 启动服务
+        if (setup_serial() && connect_ntrip()) {
+            running_ = true;
+            start_server();
+        }
+    }
+    
+    ~SerialNtripNmea()
+    {
+        running_ = false;
+        if (serial_fd_ >= 0) close(serial_fd_);
+        if (ntrip_socket_ >= 0) close(ntrip_socket_);
+    }
+
+private:
+    std::string serial_port_;
+    int baud_rate_;
+    std::string ntrip_host_;
+    int ntrip_port_;
+    std::string mountpoint_;
+    std::string username_;
+    std::string password_;
+    std::string nmea_topic_;
+    std::string frame_id_;
+    
+    int serial_fd_{-1};
+    int ntrip_socket_{-1};
+    rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;
+    std::atomic<bool> running_{false};
+    std::atomic<bool> ntrip_connected_{false};
+    
+    bool setup_serial()
+    {
+        serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+        if (serial_fd_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "串口打开失败: %s - %s", 
+                         serial_port_.c_str(), strerror(errno));
+            return false;
+        }
+        
+        struct termios tty;
+        if (tcgetattr(serial_fd_, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "获取串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 设置波特率
+        speed_t speed;
+        switch (baud_rate_) {
+            case 9600: speed = B9600; break;
+            case 19200: speed = B19200; break;
+            case 38400: speed = B38400; break;
+            case 57600: speed = B57600; break;
+            case 115200: speed = B115200; break;
+            case 230400: speed = B230400; break;
+            default:
+                RCLCPP_ERROR(this->get_logger(), "不支持的波特率: %d", baud_rate_);
+                return false;
+        }
+        
+        cfsetospeed(&tty, speed);
+        cfsetispeed(&tty, speed);
+        
+        // 设置串口参数
+        tty.c_cflag &= ~PARENB;
+        tty.c_cflag &= ~CSTOPB;
+        tty.c_cflag &= ~CSIZE;
+        tty.c_cflag |= CS8;
+        tty.c_cflag &= ~CRTSCTS;
+        tty.c_cflag |= (CREAD | CLOCAL);
+        
+        tty.c_lflag = 0;
+        tty.c_iflag = 0;
+        tty.c_oflag = 0;
+        
+        tty.c_cc[VTIME] = 0;
+        tty.c_cc[VMIN] = 0;
+        
+        if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "设置串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        tcflush(serial_fd_, TCIOFLUSH);
+        RCLCPP_INFO(this->get_logger(), "串口连接成功: %s", serial_port_.c_str());
+        return true;
+    }
+    
+    bool connect_ntrip()
+    {
+        ntrip_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (ntrip_socket_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "创建NTRIP socket失败: %s", strerror(errno));
+            return false;
+        }
+        
+        struct sockaddr_in ntrip_addr;
+        ntrip_addr.sin_family = AF_INET;
+        ntrip_addr.sin_port = htons(ntrip_port_);
+        ntrip_addr.sin_addr.s_addr = inet_addr(ntrip_host_.c_str());
+        
+        if (connect(ntrip_socket_, (struct sockaddr*)&ntrip_addr, sizeof(ntrip_addr)) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "连接NTRIP服务器失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 构建NTRIP请求
+        std::string auth_string = username_ + ":" + password_;
+        std::string auth_b64 = base64_encode(auth_string);
+        
+        std::stringstream request;
+        request << "GET /" << mountpoint_ << " HTTP/1.1\r\n";
+        request << "User-Agent: NTRIP SerialNtripNmea/1.0\r\n";
+        request << "Authorization: Basic " << auth_b64 << "\r\n";
+        request << "Accept: gnss/data\r\n";
+        request << "Connection: close\r\n";
+        request << "\r\n";
+        
+        std::string request_str = request.str();
+        
+        RCLCPP_INFO(this->get_logger(), "发送NTRIP请求到服务器: %s:%d", ntrip_host_.c_str(), ntrip_port_);
+        
+        ssize_t sent = send(ntrip_socket_, request_str.c_str(), request_str.length(), 0);
+        if (sent < 0) {
+            RCLCPP_ERROR(this->get_logger(), "发送NTRIP请求失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 读取响应
+        char buffer[1024];
+        ssize_t received = recv(ntrip_socket_, buffer, sizeof(buffer) - 1, 0);
+        if (received > 0) {
+            buffer[received] = '\0';
+            std::string response(buffer);
+            
+            if (response.find("200") != std::string::npos) {
+                ntrip_connected_ = true;
+                RCLCPP_INFO(this->get_logger(), "NTRIP连接成功: %s", mountpoint_.c_str());
+                return true;
+            } else {
+                RCLCPP_ERROR(this->get_logger(), "NTRIP连接失败: %s", response.c_str());
+            }
+        } else {
+            RCLCPP_ERROR(this->get_logger(), "未收到NTRIP服务器响应");
+        }
+        
+        return false;
+    }
+    
+    void start_server()
+    {
+        // 启动串口数据读取和NTRIP发送线程
+        std::thread serial_thread(&SerialNtripNmea::read_serial_and_process, this);
+        serial_thread.detach();
+        
+        // 启动NTRIP数据接收和写入串口线程
+        std::thread ntrip_thread(&SerialNtripNmea::read_ntrip_and_write_serial, this);
+        ntrip_thread.detach();
+        
+        // 主循环 - 显示状态信息
+        rclcpp::Rate rate(1); // 1Hz
+        while (rclcpp::ok() && running_) {
+            RCLCPP_INFO(this->get_logger(), "运行状态 - NTRIP连接: %s", ntrip_connected_ ? "是" : "否");
+            rate.sleep();
+        }
+    }
+    
+    void read_serial_and_process()
+    {
+        std::vector<char> buffer(1024);
+        std::string nmea_buffer;
+        
+        RCLCPP_INFO(this->get_logger(), "开始读取串口数据并处理");
+        
+        while (rclcpp::ok() && running_) {
+            ssize_t bytes_read = read(serial_fd_, buffer.data(), buffer.size());
+            if (bytes_read > 0) {
+                // 处理接收到的数据
+                for (ssize_t i = 0; i < bytes_read; ++i) {
+                    char c = buffer[i];
+                    nmea_buffer += c;
+                    
+                    // 检测NMEA语句结束符（换行符）
+                    if (c == '\n') {
+                        // 移除换行符和回车符
+                        if (!nmea_buffer.empty()) {
+                            // 先移除可能的换行符 '\n'
+                            if (nmea_buffer.back() == '\n') {
+                                nmea_buffer.pop_back();
+                                // 再移除可能的回车符 '\r'
+                                if (!nmea_buffer.empty() && nmea_buffer.back() == '\r') {
+                                    nmea_buffer.pop_back();
+                                }
+                            } else if (nmea_buffer.back() == '\r') {
+                                // 如果只有回车符，直接移除
+                                nmea_buffer.pop_back();
+                            }
+                        }
+                        
+                        // 检查是否为有效的NMEA语句（以$开头）
+                        if (!nmea_buffer.empty() && nmea_buffer[0] == '$') {
+                            // 发布NMEA消息到ROS话题
+                            auto nmea_msg = nmea_msgs::msg::Sentence();
+                            nmea_msg.header.frame_id = frame_id_;
+                            nmea_msg.header.stamp = this->now();
+                            nmea_msg.sentence = nmea_buffer;
+                            
+                            nmea_pub_->publish(nmea_msg);
+                            RCLCPP_DEBUG(this->get_logger(), "发布NMEA消息: %s", nmea_buffer.c_str());
+                            
+                            // 检查是否为GGA语句，发送到NTRIP服务器
+                            bool is_gga = false;
+                            if (nmea_buffer.length() >= 6) {
+                                std::string nmea_type = nmea_buffer.substr(1, 5);
+                                if (nmea_type == "GPGGA" || nmea_type == "GNGGA") {
+                                    is_gga = true;
+                                }
+                            }
+                            
+                            if (is_gga && ntrip_connected_) {
+                                // 发送GGA数据到NTRIP服务器
+                                ssize_t sent = send(ntrip_socket_, nmea_buffer.c_str(), nmea_buffer.length(), 
+                                                   MSG_DONTWAIT | MSG_NOSIGNAL);
+                                if (sent > 0) {
+                                    RCLCPP_DEBUG(this->get_logger(), "发送GGA数据到NTRIP: %s", nmea_buffer.c_str());
+                                } else if (sent < 0) {
+                                    if (errno == EPIPE || errno == ECONNRESET) {
+                                        RCLCPP_ERROR(this->get_logger(), "NTRIP连接断开");
+                                        ntrip_connected_ = false;
+                                    }
+                                }
+                            }
+                        }
+                        
+                        nmea_buffer.clear();
+                    }
+                }
+                
+                RCLCPP_DEBUG(this->get_logger(), "从串口接收到 %zd bytes 数据", bytes_read);
+                
+            } else if (bytes_read < 0) {
+                if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                    usleep(10000); // 10ms轮询间隔
+                } else {
+                    RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
+                    break;
+                }
+            } else {
+                usleep(10000); // 10ms轮询间隔
+            }
+        }
+    }
+    
+    void read_ntrip_and_write_serial()
+    {
+        std::vector<char> buffer(4096);
+        
+        while (rclcpp::ok() && running_ && ntrip_connected_) {
+            ssize_t bytes_read = recv(ntrip_socket_, buffer.data(), buffer.size(), MSG_DONTWAIT);
+            if (bytes_read > 0) {
+                // 将NTRIP返回的数据写入串口
+                ssize_t bytes_written = write(serial_fd_, buffer.data(), bytes_read);
+                if (bytes_written > 0) {
+                    RCLCPP_DEBUG(this->get_logger(), "写入串口NTRIP数据: %zd bytes", bytes_written);
+                } else if (bytes_written < 0) {
+                    if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                        RCLCPP_WARN(this->get_logger(), "串口写入缓冲区满");
+                    } else {
+                        RCLCPP_ERROR(this->get_logger(), "串口写入错误: %s", strerror(errno));
+                    }
+                }
+                
+            } else if (bytes_read == 0) {
+                RCLCPP_ERROR(this->get_logger(), "NTRIP服务器断开连接");
+                ntrip_connected_ = false;
+                break;
+            } else {
+                if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                    usleep(10000); // 10ms轮询间隔
+                } else {
+                    RCLCPP_ERROR(this->get_logger(), "NTRIP读取错误: %s", strerror(errno));
+                    ntrip_connected_ = false;
+                    break;
+                }
+            }
+        }
+        
+        // 尝试重新连接
+        if (running_ && !ntrip_connected_) {
+            RCLCPP_INFO(this->get_logger(), "尝试重新连接NTRIP服务器...");
+            usleep(5000000); // 5秒后重试
+            if (connect_ntrip()) {
+                std::thread ntrip_thread(&SerialNtripNmea::read_ntrip_and_write_serial, this);
+                ntrip_thread.detach();
+            }
+        }
+    }
+    
+    std::string base64_encode(const std::string& input)
+    {
+        static const std::string base64_chars = 
+            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+            "abcdefghijklmnopqrstuvwxyz"
+            "0123456789+/";
+            
+        std::string encoded;
+        int i = 0;
+        int j = 0;
+        unsigned char char_array_3[3];
+        unsigned char char_array_4[4];
+        
+        for (char c : input) {
+            char_array_3[i++] = c;
+            if (i == 3) {
+                char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+                char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+                char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+                char_array_4[3] = char_array_3[2] & 0x3f;
+                
+                for(i = 0; i < 4; i++)
+                    encoded += base64_chars[char_array_4[i]];
+                i = 0;
+            }
+        }
+        
+        if (i) {
+            for(j = i; j < 3; j++)
+                char_array_3[j] = '\0';
+            
+            char_array_4[0] = (char_array_3[0] & 0xfc) >> 2;
+            char_array_4[1] = ((char_array_3[0] & 0x03) << 4) + ((char_array_3[1] & 0xf0) >> 4);
+            char_array_4[2] = ((char_array_3[1] & 0x0f) << 2) + ((char_array_3[2] & 0xc0) >> 6);
+            char_array_4[3] = char_array_3[2] & 0x3f;
+            
+            for (j = 0; j < i + 1; j++)
+                encoded += base64_chars[char_array_4[j]];
+            
+            while(i++ < 3)
+                encoded += '=';
+        }
+        
+        return encoded;
+    }
+};
+
+int main(int argc, char** argv)
+{
+    // 忽略SIGPIPE信号
+    signal(SIGPIPE, SIG_IGN);
+    
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<SerialNtripNmea>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

src/serial_to_tcp_server.cpp

@@ -0,0 +1,301 @@
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <cstring>
+#include <cerrno>
+#include <vector>
+#include <thread>
+#include <atomic>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+
+class SerialToTCPServer : public rclcpp::Node
+{
+public:
+    SerialToTCPServer() : Node("serial_to_tcp_server")
+    {
+        // 声明参数
+        this->declare_parameter("serial_port", "/dev/ttyUSB0");
+        this->declare_parameter("baud_rate", 115200);
+        this->declare_parameter("tcp_host", "0.0.0.0");
+        this->declare_parameter("tcp_port", 62002);
+        
+        // 获取参数
+        serial_port_ = this->get_parameter("serial_port").as_string();
+        baud_rate_ = this->get_parameter("baud_rate").as_int();
+        tcp_host_ = this->get_parameter("tcp_host").as_string();
+        tcp_port_ = this->get_parameter("tcp_port").as_int();
+        
+        RCLCPP_INFO(this->get_logger(), "SerialToTCPServer初始化:");
+        RCLCPP_INFO(this->get_logger(), "  串口: %s @ %d baud", serial_port_.c_str(), baud_rate_);
+        RCLCPP_INFO(this->get_logger(), "  TCP服务器: %s:%d", tcp_host_.c_str(), tcp_port_);
+        
+        // 启动服务器
+        if (setup_serial() && setup_tcp_server()) {
+            running_ = true;
+            start_server();
+        }
+    }
+    
+    ~SerialToTCPServer()
+    {
+        running_ = false;
+        if (serial_fd_ >= 0) close(serial_fd_);
+        if (tcp_socket_ >= 0) close(tcp_socket_);
+        for (auto& client : client_sockets_) {
+            if (client >= 0) close(client);
+        }
+    }
+
+private:
+    std::string serial_port_;
+    int baud_rate_;
+    std::string tcp_host_;
+    int tcp_port_;
+    
+    int serial_fd_{-1};
+    int tcp_socket_{-1};
+    std::vector<int> client_sockets_;
+    std::atomic<bool> running_{false};
+    std::mutex clients_mutex_;
+    
+    bool setup_serial()
+    {
+        serial_fd_ = open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+        if (serial_fd_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "串口打开失败: %s - %s", 
+                         serial_port_.c_str(), strerror(errno));
+            return false;
+        }
+        
+        struct termios tty;
+        if (tcgetattr(serial_fd_, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "获取串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        // 设置波特率
+        speed_t speed;
+        switch (baud_rate_) {
+            case 9600: speed = B9600; break;
+            case 19200: speed = B19200; break;
+            case 38400: speed = B38400; break;
+            case 57600: speed = B57600; break;
+            case 115200: speed = B115200; break;
+            case 230400: speed = B230400; break;
+            default:
+                RCLCPP_ERROR(this->get_logger(), "不支持的波特率: %d", baud_rate_);
+                return false;
+        }
+        
+        cfsetospeed(&tty, speed);
+        cfsetispeed(&tty, speed);
+        
+        // 设置串口参数
+        tty.c_cflag &= ~PARENB; // 无奇偶校验
+        tty.c_cflag &= ~CSTOPB; // 1位停止位
+        tty.c_cflag &= ~CSIZE;
+        tty.c_cflag |= CS8;     // 8位数据位
+        tty.c_cflag &= ~CRTSCTS; // 无硬件流控
+        tty.c_cflag |= (CREAD | CLOCAL);
+        
+        tty.c_lflag = 0;
+        tty.c_iflag = 0;
+        tty.c_oflag = 0;
+        
+        tty.c_cc[VTIME] = 0;
+        tty.c_cc[VMIN] = 0;
+        
+        if (tcsetattr(serial_fd_, TCSANOW, &tty) != 0) {
+            RCLCPP_ERROR(this->get_logger(), "设置串口属性失败: %s", strerror(errno));
+            return false;
+        }
+        
+        tcflush(serial_fd_, TCIOFLUSH);
+        RCLCPP_INFO(this->get_logger(), "串口连接成功: %s", serial_port_.c_str());
+        return true;
+    }
+    
+    bool setup_tcp_server()
+    {
+        tcp_socket_ = socket(AF_INET, SOCK_STREAM, 0);
+        if (tcp_socket_ < 0) {
+            RCLCPP_ERROR(this->get_logger(), "创建TCP socket失败: %s", strerror(errno));
+            return false;
+        }
+        
+        int opt = 1;
+        setsockopt(tcp_socket_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+        
+        struct sockaddr_in server_addr;
+        server_addr.sin_family = AF_INET;
+        server_addr.sin_addr.s_addr = inet_addr(tcp_host_.c_str());
+        server_addr.sin_port = htons(tcp_port_);
+        
+        if (bind(tcp_socket_, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "TCP绑定失败: %s", strerror(errno));
+            return false;
+        }
+        
+        if (listen(tcp_socket_, 5) < 0) {
+            RCLCPP_ERROR(this->get_logger(), "TCP监听失败: %s", strerror(errno));
+            return false;
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "TCP服务器启动成功: %s:%d", tcp_host_.c_str(), tcp_port_);
+        return true;
+    }
+    
+    void start_server()
+    {
+        // 启动客户端接受线程
+        std::thread accept_thread(&SerialToTCPServer::accept_clients, this);
+        accept_thread.detach();
+        
+        // 启动数据转发线程
+        std::thread forward_thread(&SerialToTCPServer::read_serial_and_forward, this);
+        forward_thread.detach();
+        
+        // 主循环 - 显示状态信息
+        rclcpp::Rate rate(1); // 1Hz
+        while (rclcpp::ok() && running_) {
+            {
+                std::lock_guard<std::mutex> lock(clients_mutex_);
+                RCLCPP_INFO(this->get_logger(), "运行状态 - 连接客户端: %zu", client_sockets_.size());
+            }
+            rate.sleep();
+        }
+    }
+    
+    void accept_clients()
+    {
+        while (rclcpp::ok() && running_) {
+            struct sockaddr_in client_addr;
+            socklen_t client_len = sizeof(client_addr);
+            
+            int client_socket = accept(tcp_socket_, (struct sockaddr*)&client_addr, &client_len);
+            if (client_socket >= 0) {
+                // 设置非阻塞模式
+                int flags = fcntl(client_socket, F_GETFL, 0);
+                fcntl(client_socket, F_SETFL, flags | O_NONBLOCK);
+                
+                {
+                    std::lock_guard<std::mutex> lock(clients_mutex_);
+                    client_sockets_.push_back(client_socket);
+                }
+                
+                RCLCPP_INFO(this->get_logger(), "客户端连接: %s:%d", 
+                           inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+                
+                // 启动客户端处理线程
+                std::thread client_thread(&SerialToTCPServer::handle_client, this, client_socket, client_addr);
+                client_thread.detach();
+            } else {
+                if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                    RCLCPP_ERROR(this->get_logger(), "接受客户端连接错误: %s", strerror(errno));
+                }
+                usleep(100000); // 100ms延迟
+            }
+        }
+    }
+    
+    void handle_client(int client_socket, struct sockaddr_in client_addr)
+    {
+        while (rclcpp::ok() && running_) {
+            // 检查客户端是否仍然连接
+            char test_buffer[1];
+            ssize_t result = send(client_socket, test_buffer, 0, MSG_DONTWAIT);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                break;
+            }
+            usleep(100000); // 100ms延迟
+        }
+        
+        close(client_socket);
+        {
+            std::lock_guard<std::mutex> lock(clients_mutex_);
+            auto it = std::find(client_sockets_.begin(), client_sockets_.end(), client_socket);
+            if (it != client_sockets_.end()) {
+                client_sockets_.erase(it);
+            }
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "客户端断开: %s:%d", 
+                   inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
+    }
+    
+    void read_serial_and_forward()
+    {
+        std::vector<char> buffer(4096);
+        std::string nmea_buffer;
+        
+        while (rclcpp::ok() && running_) {
+            ssize_t bytes_read = read(serial_fd_, buffer.data(), buffer.size() - 1);
+            if (bytes_read > 0) {
+                buffer[bytes_read] = '\0';
+                nmea_buffer.append(buffer.data(), bytes_read);
+                
+                // 处理完整的NMEA语句
+                size_t pos = 0;
+                while ((pos = nmea_buffer.find("\r\n")) != std::string::npos) {
+                    std::string nmea_line = nmea_buffer.substr(0, pos);
+                    nmea_buffer.erase(0, pos + 2); // 移除已处理的语句和\r\n
+                    
+                    if (!nmea_line.empty() && nmea_line[0] == '$') {
+                        // 转发给所有连接的客户端
+                        forward_to_clients(nmea_line + "\r\n");
+                        
+                        static int debug_counter = 0;
+                        if (++debug_counter >= 10) {
+                            RCLCPP_DEBUG(this->get_logger(), "转发NMEA数据: %s...", 
+                                       nmea_line.substr(0, 50).c_str());
+                            debug_counter = 0;
+                        }
+                    }
+                }
+            } else if (bytes_read < 0) {
+                if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                    RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno));
+                    usleep(1000000); // 1秒后重试
+                }
+            }
+            usleep(10000); // 10ms延迟
+        }
+    }
+    
+    void forward_to_clients(const std::string& data)
+    {
+        std::lock_guard<std::mutex> lock(clients_mutex_);
+        std::vector<int> disconnected_clients;
+        
+        for (int client_socket : client_sockets_) {
+            ssize_t result = send(client_socket, data.c_str(), data.length(), MSG_DONTWAIT);
+            if (result < 0 && (errno == EPIPE || errno == ECONNRESET)) {
+                disconnected_clients.push_back(client_socket);
+            }
+        }
+        
+        // 移除断开的客户端
+        for (int client : disconnected_clients) {
+            auto it = std::find(client_sockets_.begin(), client_sockets_.end(), client);
+            if (it != client_sockets_.end()) {
+                close(client);
+                client_sockets_.erase(it);
+            }
+        }
+    }
+};
+
+int main(int argc, char** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<SerialToTCPServer>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}