###### tags: `控制組`

# socket in ROS

Example code to interface socket with ros

#include "ros/ros.h"
#include "std_msgs/Int32MultiArray.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>

int main(int argc, char** argv) {
  // Initialize the ROS node
  ros::init(argc, argv, "test_publisher");
  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<std_msgs::Int32MultiArray>("test_value", 10);
  std_msgs::Int32MultiArray msg;

  // Set the size of the array
  msg.data.resize(3);

  msg.data[0] = 0;
  msg.data[1] = 10;
  msg.data[2] = 0;

  // Publish the message at a rate of 1 Hz
  ros::Rate rate(1);
  ros::spinOnce();
  
  // initialization
  int server_fd, new_socket, valread;
  struct sockaddr_in address;
  int opt = 1;
  int addrlen = sizeof(address);
  unsigned char buffer[1024] = { 0 };
  // Create the server socket
  int serverSocket = socket(AF_INET, SOCK_STREAM, 0);
  if (serverSocket == -1) {
    ROS_ERROR("Failed to create server socket");
    return 1;
  }

  // Set up the server address
  struct sockaddr_in serverAddress;
  serverAddress.sin_family = AF_INET;
  serverAddress.sin_port = htons(42069);
  serverAddress.sin_addr.s_addr = INADDR_ANY;

  // Bind the server socket to the address
  if (bind(serverSocket, (struct sockaddr*)&serverAddress, sizeof(serverAddress)) == -1) {
    ROS_ERROR("Failed to bind server socket");
    close(serverSocket);
    return 1;
  }

  // Listen for incoming connections
  if (listen(serverSocket, 10) == -1) {
    ROS_ERROR("Failed to listen on server socket");
    close(serverSocket);
    return 1;
  }

  ROS_INFO("Listening...");

  while (ros::ok()) {
    // Accept client connection
    int clientSocket = accept(serverSocket, (struct sockaddr*)&address, (socklen_t*)&addrlen);
    if (clientSocket == -1) {
      ROS_ERROR("Failed to accept client connection");
      close(serverSocket);
      return 1;
    }
    // Handle client request
    int bytesRecv;
    unsigned char tmp;

    if ((bytesRecv = read(clientSocket, buffer, 1024)) == -1) {
        ROS_ERROR("failed: %s", strerror(errno));
    }

    ROS_INFO("Recvd %d bytes", bytesRecv);

    tmp = buffer[0];

    msg.data[0] = tmp & 0x3; tmp >>= 2;
    msg.data[1] = tmp & 0x1; tmp >>= 1;
    msg.data[2] = tmp & 0xf;
    
    //ROS_INFO("Data: %d, %d, %d", msg.data[0], msg.data[1], msg.data[2]);

    pub.publish(msg);

  }
  // Close server socket
  close(serverSocket);

  return 0;
}

- testing

import socket

host = "127.0.0.1"
port = 42069

s = socket.socket()
s.connect((host, port))
seq = "\x6E".encode()
s.send(seq)

- rviz problem