C 语言

关注公众号 jb51net

关闭
首页 > 软件编程 > C 语言 > C++ TCP通讯

C++实现本地TCP通讯的示例代码

作者:点PY

这篇文章主要为大家详细介绍了C++如何利用TCP技术,实现本地ROS1和ROS2的通讯,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下

概要

利用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;
}

到此这篇关于C++实现本地TCP通讯的示例代码的文章就介绍到这了,更多相关C++ TCP通讯内容请搜索脚本之家以前的文章或继续浏览下面的相关文章希望大家以后多多支持脚本之家!

您可能感兴趣的文章:
阅读全文