puai_wj_2021/velodyne_lidar/velodyne_driver/input.cc

// Copyright (C) 2007, 2009, 2010, 2015 Austin Robot Technology, Patrick Beeson, Jack O'Quin
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above
//    copyright notice, this list of conditions and the following
//    disclaimer in the documentation and/or other materials provided
//    with the distribution.
//  * Neither the name of {copyright_holder} nor the names of its
//    contributors may be used to endorse or promote products derived
//    from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

/** \file
 *
 *  Input classes for the Velodyne HDL-64E 3D LIDAR:
 *
 *     Input -- base class used to access the data independently of
 *              its source
 *
 *     InputSocket -- derived class reads live data from the device
 *              via a UDP socket
 *
 *     InputPCAP -- derived class provides a similar interface from a
 *              PCAP dump
 */

#include <unistd.h>
#include <string>
#include <sstream>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <poll.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/file.h>

#include "input.h"

namespace velodyne_driver
{
  ////////////////////////////////////////////////////////////////////////
  // InputSocket class implementation
  ////////////////////////////////////////////////////////////////////////

  bool InputSocket::init(std::string ip, uint16_t port)
  {
    m_port = port;
    m_devip_str = ip;
    m_sockfd = -1;

    if (!m_devip_str.empty())
    {
      inet_aton(m_devip_str.c_str(), &m_devip);
    }

    // connect to Velodyne UDP port
    LOG(INFO) << "Opening UDP socket: port " << m_port;
    m_sockfd = socket(PF_INET, SOCK_DGRAM, 0);
    if (m_sockfd == -1)
    {
      perror("socket"); // TODO: ROS_ERROR errno
      return false;
    }

    sockaddr_in my_addr;                  // my address information
    memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
    my_addr.sin_family = AF_INET;         // host byte order
    my_addr.sin_port = htons(m_port);       // port in network byte order
    my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP

    if (bind(m_sockfd, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
    {
      perror("bind"); // TODO: ROS_ERROR errno
      return false;
    }

    if (fcntl(m_sockfd, F_SETFL, O_NONBLOCK | FASYNC) < 0)
    {
      perror("non-block");
      return false;
    }

    LOG(INFO) << "Velodyne socket fd is " << m_sockfd;
    return true;
  }

  void InputSocket::uninit()
  {
    (void)close(m_sockfd);
  }

  /** @brief Get one velodyne packet. */
  int InputSocket::getPacket(unsigned char* pkt)
  {
    struct pollfd fds[1];
    fds[0].fd = m_sockfd;
    fds[0].events = POLLIN;
    static const int POLL_TIMEOUT = 1000; // one second (in msec)

    sockaddr_in sender_address;
    socklen_t sender_address_len = sizeof(sender_address);

    while (true)
    {
      // Unfortunately, the Linux kernel recvfrom() implementation
      // uses a non-interruptible sleep() when waiting for data,
      // which would cause this method to hang if the device is not
      // providing data.  We poll() the device first to make sure
      // the recvfrom() will not block.
      //
      // Note, however, that there is a known Linux kernel bug:
      //
      //   Under Linux, select() may report a socket file descriptor
      //   as "ready for reading", while nevertheless a subsequent
      //   read blocks.  This could for example happen when data has
      //   arrived but upon examination has wrong checksum and is
      //   discarded.  There may be other circumstances in which a
      //   file descriptor is spuriously reported as ready.  Thus it
      //   may be safer to use O_NONBLOCK on sockets that should not
      //   block.

      // poll() until input available
      do
      {
        int retval = poll(fds, 1, POLL_TIMEOUT);
        if (retval < 0) // poll() error?
        {
          if (errno != EINTR)
            LOG(ERROR) << m_port << " poll() error:" << strerror(errno);
          return -1;
        }
        if (retval == 0) // poll() timeout?
        {
          LOG_EVERY_N(WARNING, 3000) << "Velodyne poll() timeout "<<m_port;
          return -1;
        }
        if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
        {
          LOG(ERROR) << "poll() reports Velodyne error " << m_port;
          return -1;
        }
      } while ((fds[0].revents & POLLIN) == 0);

      // Receive packets that should now be available from the
      // socket using a blocking read.
      ssize_t nbytes = recvfrom(m_sockfd, pkt,
                                PACKET_SIZE, 0,
                                (sockaddr *)&sender_address,
                                &sender_address_len);

      if (nbytes < 0)
      {
        if (errno != EWOULDBLOCK)
        {
          perror("recvfail");
          LOG(INFO) << "recvfail";
          return -1;
        }
      }
      else if ((size_t)nbytes == PACKET_SIZE)
      {
        // read successful,
        // if packet is not from the lidar scanner we selected by IP,
        // continue otherwise we are done
        if (m_devip_str != "" && sender_address.sin_addr.s_addr != m_devip.s_addr)
          continue;
        else
        {
          // memcpy(&packet, pkt, PACKET_SIZE);
          break; //done
        }
      }

      LOG(INFO) << "incomplete Velodyne packet read: " << nbytes << " bytes";
    }

    return 0;
  }

} // velodyne namespace