概要
利用TCP技术，实现本地ROS1和ROS2的通讯。
服务端代码
头文件
#include <ros/ros.h>
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include "geometry_msgs/Twist.h"using namespace std;class TCPPublisher
{public:TCPPublisher();~TCPPublisher();void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);private:ros::NodeHandle n, nPrivate;ros::Publisher tcpPub;ros::Subscriber cmdVelSub;// 创建服务器套接字int serverSocket;// 设置服务器地址结构sockaddr_in serverAddr;int clientSocket;std::string topicStatus;
};
源代码
#include "./tcp_pub/tcp_pub.h"TCPPublisher::TCPPublisher():nPrivate("~")
{nPrivate.param("topicStatus", topicStatus, std::string("/cmd_vel"));/*订阅话题*/cmdVelSub = n.subscribe(topicStatus.c_str(), 10, &TCPPublisher::cmdVelCallback, this);serverSocket = socket(AF_INET, SOCK_STREAM, 0);// 设置服务器地址结构sockaddr_in serverAddr;serverAddr.sin_family = AF_INET;serverAddr.sin_addr.s_addr = INADDR_ANY;serverAddr.sin_port = htons(8080); // 服务器监听的端口号// 绑定套接字if (bind(serverSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == -1) {std::cerr << "Bind failed." << std::endl;close(serverSocket);return;}// 监听连接if (listen(serverSocket, SOMAXCONN) == -1) {std::cerr << "Listen failed." << std::endl;close(serverSocket);return;}std::cout << "Server is listening for incoming connections..." << std::endl;ROS_INFO("TCPPublisher init successfully!!!");
}TCPPublisher::~TCPPublisher()
{close(serverSocket);
}void TCPPublisher::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{float velX = msg->linear.x;float angularZ = msg->angular.z;// ROS_INFO("velX : %f, angularZ : %f", velX, angularZ);// 定义字符数组，用于存储转换后的结果char buffer[50]; // 适当调整数组大小以适应你的需求snprintf(buffer, sizeof(buffer), "%f,%f", velX, angularZ);// ROS_INFO("buffer %s", buffer);// 接受连接int clientSocket = accept(serverSocket, NULL, NULL);if (clientSocket == -1) {std::cerr << "Accept failed." << std::endl;close(serverSocket);return;}std::cout << "Connection established with a client." << std::endl;// 发送消息给客户端const char* message = buffer;ROS_INFO("message %s", message);if (send(clientSocket, message, strlen(message), 0) == -1) {std::cerr << "Error sending message." << std::endl;}// 关闭客户端套接字close(clientSocket);
}int main(int argc, char **argv) {//创建节点ros::init(argc, argv, "pure_pursuit");TCPPublisher tp; ros::spin();return 0;
}
客户端代码
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <iostream>
#include <cstring>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>int main(int argc, char * argv[]) {rclcpp::init(argc, argv);auto node = std::make_shared<rclcpp::Node>("tcp_client");/*define publisher*/rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;// Advertise velocity commandsauto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());cmd_pub_ = node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", default_qos);//连接到服务器// 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;while (true) {// 创建客户端套接字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(8080); // 服务器监听的端口号// 连接到服务器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 floatValue1, floatValue2;// 使用 sscanf 解析字符数组if (std::sscanf(buffer, "%f,%f", &floatValue1, &floatValue2) == 2) {// 打印解析结果std::cout << "解析后的浮点数1: " << floatValue1 << std::endl;std::cout << "解析后的浮点数2: " << floatValue2 << std::endl;} else {// 解析失败std::cerr << "解析失败" << std::endl;}geometry_msgs::msg::Twist cmd_msg;cmd_msg.linear.x = floatValue1;cmd_msg.angular.z = floatValue2;cmd_pub_->publish(cmd_msg);std::cout << "Publishing zero speed to /cmd_vel. " << std::endl;}// 关闭客户端套接字close(clientSocket);// 在这里可以添加一些延时，以避免过于频繁地连接服务器sleep(0.1);}rclcpp::spin(node);rclcpp::shutdown();return 0;
}
