利用TCP发布GNSS数据（ROS2转ROS1）

本文主要是介绍利用TCP发布GNSS数据（ROS2转ROS1），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

文章目录

服务端（ROS2）
客户端（ROS1）

服务端（ROS2）

#include <memory>
#include <iostream>
#include <sstream>
#include <string>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <cstring>
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include <sensor_msgs/msg/nav_sat_fix.hpp>class GNSSSubscriber : public rclcpp::Node
{
public:GNSSSubscriber() : Node("GNSS_subscriber"), socket_fd_(-1){// 创建TCP服务器端socketsocket_fd_ = socket(AF_INET, SOCK_STREAM, 0);if (socket_fd_ == -1) {RCLCPP_ERROR(this->get_logger(), "Failed to create socket.");rclcpp::shutdown();}sockaddr_in server_addr;server_addr.sin_family = AF_INET;server_addr.sin_port = htons(8889);server_addr.sin_addr.s_addr = INADDR_ANY;if (bind(socket_fd_, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {RCLCPP_ERROR(this->get_logger(), "Failed to bind to port 8888.");rclcpp::shutdown();}listen(socket_fd_, 5); // 监听，最多5个等待连接的队列// ROS 2 订阅里程计消息subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("/gnss_wgs84", 10, std::bind(&GNSSSubscriber::gnss_callback, this, std::placeholders::_1));}~GNSSSubscriber(){if (socket_fd_ != -1) {close(socket_fd_);}}private:void gnss_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg){// 等待并接受一个新连接sockaddr_in client_addr;socklen_t client_len = sizeof(client_addr);int client_socket = accept(socket_fd_, (struct sockaddr *)&client_addr, &client_len);if (client_socket < 0) {RCLCPP_ERROR(this->get_logger(), "Failed to accept connection.");return;}// 将里程计消息转换为字符串std::ostringstream ss;ss << msg->latitude << ", " << msg->longitude << ", " << msg->altitude << ", "<< static_cast<float>(msg->status.status);std::string odom_message = ss.str();std::cout << "message: " << odom_message << std::endl;// 发送里程计消息给客户端send(client_socket, odom_message.c_str(), odom_message.size(), 0);// 关闭客户端socketclose(client_socket);}rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr subscription_;nav_msgs::msg::Odometry last_odom_;int socket_fd_;
};int main(int argc, char *argv[])
{rclcpp::init(argc, argv);auto node = std::make_shared<GNSSSubscriber>();rclcpp::spin(node);rclcpp::shutdown();return 0;
}

客户端（ROS1）

#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>int main(int argc, char **argv) {// 初始化ROS节点ros::init(argc, argv, "gps_publisher");ros::NodeHandle nh;// 创建一个发布者ros::Publisher gps_pub = nh.advertise<sensor_msgs::NavSatFix>("gps_data", 1000);ros::Rate loop_rate(1);  // 设置发布频率为1 Hzwhile (ros::ok()) {sensor_msgs::NavSatFix msg;// 创建客户端套接字int clientSocket = socket(AF_INET, SOCK_STREAM, 0);if (clientSocket == -1) {std::cerr << "Failed to create client socket." << std::endl;return 1;}// 设置服务器地址结构sockaddr_in serverAddr;serverAddr.sin_family = AF_INET;serverAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); // 本地回环地址 // 服务器的 IP 地址serverAddr.sin_port = htons(8889); // 服务器监听的端口号// 连接到服务器if (connect(clientSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {std::cerr << "Connection failed." << std::endl;close(clientSocket);// return 1;}// std::cout << "Connected to the server." << std::endl;// 接收消息char buffer[50];memset(buffer, 0, sizeof(buffer));if (recv(clientSocket, buffer, sizeof(buffer), 0) == -1) {std::cerr << "Error receiving message." << std::endl;} else {std::cout << "Received message from server: " << buffer << std::endl;// 定义两个变量来存储解析后的浮点数float lat, lon, alt, status;// 使用 sscanf 解析字符数组if (std::sscanf(buffer, "%f,%f,%f,%f", &lat, &lon, &alt, &status) == 4) {// 打印解析结果std::cout << "解析后的数据: " << lat << ", " << lon << ", " << alt << ", " << status << std::endl;msg.latitude = lat;msg.longitude = lon;msg.altitude = alt;msg.status.status = static_cast<int8_t>(status);// 填充协方差矩阵msg.position_covariance[0] = 0.02;  // 经度方差msg.position_covariance[1] = 0.0;  // 经度与纬度的协方差msg.position_covariance[2] = 0.0;  // 经度与高度的协方差msg.position_covariance[3] = 0.0;  // 纬度与经度的协方差msg.position_covariance[4] = 0.02;  // 纬度方差msg.position_covariance[5] = 0.0;  // 纬度与高度的协方差msg.position_covariance[6] = 0.0;  // 高度与经度的协方差msg.position_covariance[7] = 0.0;  // 高度与纬度的协方差msg.position_covariance[8] = 0.02;  // 高度方差gps_pub.publish(msg);} else {// 解析失败std::cerr << "解析失败" << std::endl;}}// 关闭客户端套接字close(clientSocket);// 在这里可以添加一些延时，以避免过于频繁地连接服务器sleep(0.1);ros::spinOnce();loop_rate.sleep();}return 0;
}