本地TCP通讯（C++）

概要

利用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"usingnamespace std;classTCPPublisher{public:TCPPublisher();~TCPPublisher();voidcmdVelCallback(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,(structsockaddr*)&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);}voidTCPPublisher::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;// 发送消息给客户端constchar* message = buffer;ROS_INFO("message %s", message);if(send(clientSocket, message,strlen(message),0)==-1){
        std::cerr <<"Error sending message."<< std::endl;}// 关闭客户端套接字close(clientSocket);}intmain(int argc,char**argv){//创建节点
  ros::init(argc, argv,"pure_pursuit");
  TCPPublisher tp;  
  ros::spin();return0;}

客户端代码

#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>intmain(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;return1;}// 设置服务器地址结构
        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,(structsockaddr*)&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();return0;}