ROS——udp通信

#include "send_recv.h"
#include <type_traits>
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

CUDPClient::CUDPClient()
{  }

CUDPClient::~CUDPClient()
{ 
  close(sock_id);
}


bool CUDPClient::Init(string destIP, short destPort, short selfPort)
{

std::cout<<"------------init-------------------------"<<std::endl;

  dest_IP_address = destIP;
  dest_port = destPort;
  self_port = selfPort;
  /* udp socket */
  sock_id = socket(AF_INET, SOCK_DGRAM, 0); //创建套接字
  if(sock_id < 0)
  {
    cout<<"ERROR! socket error "<<endl;
    return false;
  }
 //将addr_serv数组全体内存空间按字节整体清零
  memset((char*)&addr_in, 0, sizeof(sockaddr_in));
  addr_in.sin_family = AF_INET;
  addr_in.sin_port = htons(self_port);
  addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
  ROS_INFO("in port:%d ",self_port);
  if(bind(sock_id, (struct sockaddr*)&addr_in, sizeof(struct sockaddr_in)) == -1 )
  {
    ROS_ERROR("bind error");
    close(sock_id);
    return false;
  }

  memset((char*)&addr_to, 0, sizeof(sockaddr_in));
  addr_to.sin_family = AF_INET;
  addr_to.sin_port = htons(dest_port);
  addr_to.sin_addr.s_addr = inet_addr(dest_IP_address.c_str());

  return true;
}

int CUDPClient::SendInfo(const char *send_buf, int bufLen)
{
   std::cout<<"------------SendInfo-------------------------"<<std::endl;

  return sendto(sock_id, send_buf, bufLen, 0, (struct sockaddr *)&addr_to, sizeof(struct sockaddr_in));

}

int CUDPClient::RecvInfo(char *recv_buf)
{
    std::cout<<"------------RecvInfo-------------------------"<<std::endl;
   
  //避免阻塞等待，设置计时 
  fd_set readFds;
  timeval time_out;
  struct sockaddr_in addr_from;
  int addr_size = sizeof(struct sockaddr_in);

  FD_ZERO(&readFds);
  FD_SET(sock_id, &readFds);
  time_out.tv_sec = 0;
  time_out.tv_usec = 1*1000;
  int res = select(sock_id + 1, &readFds, NULL, NULL, &time_out); 
  std::cout<<"res="<<res<<std::endl;
  if(res>0)
  {
    std::cout<<"res>0"<<std::endl;
    // return recvfrom(sock_id, recv_buf, RECV_BUFF_LEN, 0, (struct sockaddr *)&addr_from, (socklen_t *)&addr_size);
    return recvfrom(sock_id, recv_buf, sizeof(recv_buf), 0, (struct sockaddr *)&addr_from, (socklen_t *)&addr_size);
  }
  else
  {
     switch (errno) {
        case EBADF:
         std::cout<<"1"<<std::endl;
            // 处理无效的文件描述符错误
            break;
        case EINTR:
          std::cout<<"2"<<std::endl;
            // 处理被信号中断错误
            break;
        case EINVAL:
          std::cout<<"3"<<std::endl;
            // 处理无效的参数错误
            break;
        case ENOMEM:
        std::cout<<"4"<<std::endl;
            // 处理内存不足错误
            break;
        default:
        std::cout<<"5"<<std::endl;
            // 处理其他错误
            break;
    }
    return -1;
  }
}

bool CUDPClient::DataAnalysis(char *buff)
{
  return false;
}


int main(int argc, char** argv)
{
    ros::init(argc ,  argv, "CUDPsender");

     ros::NodeHandle n;

    CUDPClient udp_client;

    string destIP = "127.0.0.1";
    short destPort=2000;
    short selfPort=2000;

    const char *send_buf="Hello,This is Ros";
    int bufLen = strlen(send_buf);

    char recv_buf[1024]={0};
    ros::Rate loop_rate(5);//while以50Hz进行循环
 if( !udp_client.Init(destIP, destPort, selfPort))
 {
    return -1;
 }
 while(ros::ok())
{
   
    
    int send_num=udp_client.SendInfo(send_buf,bufLen);

    //判断发送是否成功
    if(send_num<=0)
    {
        perror("sendto error:");
        exit(1);
     }
    else
     {
        ROS_INFO("send successful");
    }
    

     int recvlen=udp_client.RecvInfo(recv_buf);

        //判断接收
    if(recvlen == -1)
    {
        perror("recvform error");
        // exit(1);
    }
        printf("received data: %s\n", recv_buf);


    ros::spinOnce(); 
    loop_rate.sleep();   
}

    return 0;
}

注意端口号一样，因为自己发自己收用的同一个端口