本地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"

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 commands
    auto 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;
}