文章目录
- 服务端（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服务器端socket
socket_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);
// 关闭客户端socket
close(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 Hz
while (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;
}
