modularization/src/driver/driver_radar_4d_ars548/ars548recv.cpp

#include "ars548recv.h"

ars548recv::ars548recv()
{
    InitSock();

    std::thread * pthread = new std::thread(&ars548recv::threadrecv,this);
}


void ars548recv::InitSock()
{
    std::string mcast_ip("224.0.2.2");
     uint16_t mcast_port = 42102;


     notify_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
     if (notify_fd_ == -1) {
       std::cout << "fail to create notify fd, " << strerror(errno);
       return ;
     }

     memset(&notify_addr_, 0, sizeof(notify_addr_));
     notify_addr_.sin_family = AF_INET;
     notify_addr_.sin_addr.s_addr = inet_addr(mcast_ip.c_str());
     notify_addr_.sin_port = htons(mcast_port);

     listen_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
     if (listen_fd_ == -1) {
       std::cout << "fail to create listen fd, " << strerror(errno);
       return ;
     }

     if (fcntl(listen_fd_, F_SETFL, O_NONBLOCK) == -1) {
       std::cout << "fail to set listen fd nonblock, " << strerror(errno);
       return ;
     }

     memset(&listen_addr_, 0, sizeof(listen_addr_));
     listen_addr_.sin_family = AF_INET;
     listen_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
     listen_addr_.sin_port = htons(mcast_port);

     int yes = 1;
     if (setsockopt(listen_fd_, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) < 0) {
       std::cout << "fail to setsockopt SO_REUSEADDR, " << strerror(errno);
       return ;
     }

     if (bind(listen_fd_, (struct sockaddr*)&listen_addr_, sizeof(listen_addr_)) <
         0) {
       std::cout << "fail to bind addr, " << strerror(errno);
       return ;
     }

     int loop = 1;
     if (setsockopt(listen_fd_, IPPROTO_IP, IP_MULTICAST_ALL, &loop,
                    sizeof(loop)) < 0) {
       std::cout << "fail to setsockopt IP_MULTICAST_LOOP, " << strerror(errno);
       return ;
     }

     struct ip_mreq mreq;

//     mreq.imr_interface.s_addr = htonl(INADDR_ANY);

     inet_aton( "192.168.1.102", &(mreq.imr_interface) );
     if (setsockopt(listen_fd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq,
                    sizeof(mreq)) < 0) {
       std::cout << "fail to setsockopt IP_ADD_MEMBERSHIP, " << strerror(errno);
       return ;
     }

     return ;

}

void ars548recv::threadrecv()
{
    int timeout_ms = 100;
    while(1)
    {
        int nrtn = 0;
        struct pollfd fds;
        fds.fd = listen_fd_;
        fds.events = POLLIN;
        int ready_num = poll(&fds, 1, timeout_ms);
        if (ready_num > 0) {
            char buf[32] = {0};  // larger than ReadableInfo::kSize
            ssize_t nbytes = recvfrom(listen_fd_, buf, 32, 0, nullptr, nullptr);
            if (nbytes == -1) {
                std::cout << "fail to recvfrom, " << strerror(errno)<<std::endl;
                continue;
            }
            if(nbytes >0)
            {
                std::cout<<" recv data: "<<nbytes<<std::endl;
            }
    //          return info->DeserializeFrom(buf, nbytes);
        } else if (ready_num == 0) {
            continue;
        } else {
            continue;
            if (errno == EINTR) {
                std::cout << "poll was interrupted."<<std::endl;
            } else {
                std::cout << "fail to poll, " << strerror(errno);
            }
        }


    }
}