|
|
@@ -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;
|
|
|
+}
|
DSTD
2 ماه پیش