在ROS2中用socket实现服务端和多客户端的双端通信

1.创建工作空间

mkdir -p ~/chapt/src

2.创建功能包

cd ~/chapt/src

ros2 pkg create client --build-type ament_cmake --dependencies rclcpp

ros2 pkg create server --build-type ament_cmake --dependencies rclcpp

touch ~/chapt/src/client/src/client.cpp

touch ~/chapt/src/server/src/server.cpp

3.客户端的代码实现

#include <rclcpp/rclcpp.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <thread>

#define MYPORT 8080
#define BUF_SIZE 1024

const char* SERVER_IP = "127.0.0.1";

using namespace std;

class TcpClientNode : public rclcpp::Node
{
public:
  TcpClientNode() : Node("tcp_client")
  {
    // 创建套接字
    socket_cli = socket(AF_INET, SOCK_STREAM, 0);
    if (socket_cli < 0)
    {
      RCLCPP_ERROR(this->get_logger(), "socket() error");
      exit(EXIT_FAILURE);
    }

    // 初始化服务器地址信息
    memset(&sev_addr, 0, sizeof(sev_addr));
    sev_addr.sin_family = AF_INET;
    sev_addr.sin_port = htons(MYPORT);
    sev_addr.sin_addr.s_addr = inet_addr(SERVER_IP);

    // 连接服务器
    RCLCPP_INFO(this->get_logger(), "Connecting...");
    if (connect(socket_cli, (struct sockaddr*)&sev_addr, sizeof(sev_addr)) < 0)
    {
      RCLCPP_ERROR(this->get_logger(), "Connect error");
      exit(EXIT_FAILURE);
    }
    else
    {
      RCLCPP_INFO(this->get_logger(), "Connected successfully!");
    }
  }

  void sendMessages()
  {
    char msgbuf[1024] = "你好，我是客户端!";
    while (rclcpp::ok())
    {
      send(socket_cli, msgbuf, sizeof(msgbuf), 0);
      this_thread::sleep_for(chrono::seconds(1)); // 每秒发送一次消息
    }
  }

  void receiveMessages()
  {
    char recvbuf[BUF_SIZE];
    while (rclcpp::ok())
    {
      ssize_t n = recv(socket_cli, recvbuf, sizeof(recvbuf), 0);
      if (n <= 0)
      {
        break;
      }
      // 对收到的消息进行处理
      string receivedMessage(recvbuf);
      string processedMessage = receivedMessage+"0000000";

      // 发送处理后的消息回服务端
      // send(socket_cli, processedMessage.c_str(), processedMessage.length(), 0);
      // this_thread::sleep_for(chrono::seconds(1));
      RCLCPP_INFO(this->get_logger(), "Server message: %s", recvbuf);
    }
  }

  void run()
  {
    // 创建发送和接收消息的线程
    thread sendThread(&TcpClientNode::sendMessages, this);
    thread receiveThread(&TcpClientNode::receiveMessages, this);

    // 等待线程结束
    sendThread.join();
    receiveThread.join();
  }

private:
  int socket_cli;
  struct sockaddr_in sev_addr;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TcpClientNode>();
  node->run();
  rclcpp::shutdown();
  return 0;
}

4.服务端代码

#include <rclcpp/rclcpp.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <thread>
#include <vector>

#define PORT 8080
#define LOG 10

using namespace std;

class TcpServerNode : public rclcpp::Node
{
public:
  TcpServerNode() : Node("tcp_server")
  {
    // 创建套接字
    listenfd = socket(AF_INET, SOCK_STREAM, 0);
    if (listenfd == -1)
    {
      RCLCPP_ERROR(this->get_logger(), "socket() error");
      exit(EXIT_FAILURE);
    }

    // 初始化服务器套接字地址信息
    memset(&server, 0, sizeof(server));
    server.sin_family = AF_INET;
    server.sin_addr.s_addr = htonl(INADDR_ANY);
    server.sin_port = htons(PORT);

    // 绑定套接字
    if (bind(listenfd, (struct sockaddr *)&server, sizeof(server)) < 0)
    {
      RCLCPP_ERROR(this->get_logger(), "bind() error");
      exit(EXIT_FAILURE);
    }

    // 监听套接字
    if (listen(listenfd, LOG) < 0)
    {
      RCLCPP_ERROR(this->get_logger(), "listen() error");
      exit(EXIT_FAILURE);
    }

    RCLCPP_INFO(this->get_logger(), "TCP Server started on port %d", PORT);
  }

  void run()
  {
    while (rclcpp::ok())
    {
      socklen_t clientlen = sizeof(client);
      connectfd = accept(listenfd, (struct sockaddr *)&client, &clientlen);
      if (connectfd < 0)
      {
        RCLCPP_ERROR(this->get_logger(), "accept() error");
        continue;
      }
      char client_ip[INET_ADDRSTRLEN];
      inet_ntop(AF_INET, &client.sin_addr, client_ip, INET_ADDRSTRLEN);
      RCLCPP_INFO(this->get_logger(), "Client connected from IP: %s, port: %d", client_ip, ntohs(client.sin_port));

    // 创建新线程处理客户端连接
    threads.push_back(thread(&TcpServerNode::handleClient, this, connectfd));
    
    close(connectfd);
    }
  }

  void handleClient(int connectfd)
  {
    char msgbuf[1024] = "你好，我是服务端!";
    send(connectfd, msgbuf, sizeof(msgbuf), 0);

    // 接收客户端消息
    char recvbuf[1024];
    while (rclcpp::ok())
    {
      ssize_t n = recv(connectfd, recvbuf, sizeof(recvbuf), 0);
      if (n <= 0)
      {
        break;
      }
      RCLCPP_INFO(this->get_logger(), "Received message from client: %s", recvbuf);
      send(connectfd, msgbuf, sizeof(msgbuf), 0);
    }

    close(connectfd);
  }

private:
  int listenfd, connectfd;
  struct sockaddr_in server;
  struct sockaddr_in client;
  vector<thread> threads;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TcpServerNode>();
  node->run();
  rclcpp::shutdown();
  return 0;
}

5.在两个功能包的CMakeLists.txt中分别添加以下内容

1.
add_executable(server_node src/server.cpp)
ament_target_dependencies(server_node rclcpp)

install(TARGETS
  server_node
  DESTINATION lib/${PROJECT_NAME}
)

2.
add_executable(client_node src/client.cpp)
ament_target_dependencies(client_node rclcpp)

install(TARGETS
  client_node
  DESTINATION lib/${PROJECT_NAME}
)

6.编译运行

cd ~/chapt/src

colcon build

source install/setup.bash

ros2 run server server_node

ros2 run client client_node