position_listener.h

Go to the documentation of this file.

//
// Copyright (c) 2017 Regents of the SIGNET lab, University of Padova.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Padova (SIGNET lab) 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.
//
 
#ifndef _POSITION_LISTENER_H_
#define _POSITION_LISTENER_H_
 
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
#define SOCKET_TYPE int
#define SET_SOCKOPT_TYPE void *
#define SENDTO_TYPE const char *
#define SOCKET_ERROR -1
 
#include <cstring>
#include <stdexcept>
#include <vector>
 
#include "logging.h"
#include "position_data.h"
#include "stoppable_thread.h"
 
template <typename Owner> class PositionListener : public StoppableThread
{
public:
    PositionListener(Owner *owner, uint16_t port, timeval read_timeout)
    {
        p_Owner = owner;
        m_Port = port;
        m_ReadTimeout = read_timeout;
    }
    virtual ~PositionListener()
    {
        if (m_SocketFD)
            ::close(m_SocketFD);
    }
    virtual void
    Run()
    {
        try {
            if (p_Owner->debugLevel() > 0)
                LOG_MSG_INFO(
                        "Starting position data listener on port " << m_Port);
            // set up socket....
            m_SocketFD = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
            if (m_SocketFD < 0)
                throw std::runtime_error(
                        "PositionListener::ListenLoop()::socket()");
 
            // allow to reuse the address
            int reuse = 1;
            if (setsockopt(m_SocketFD,
                        SOL_SOCKET,
                        SO_REUSEADDR,
                        (SET_SOCKOPT_TYPE) &reuse,
                        sizeof(reuse)) == -1)
                throw std::runtime_error(
                        "PositionListener::ListenLoop::setsockopt::reuse");
 
            // setup receive buffer to be large enough
            int rx_buffer_size = 1024;
            if (setsockopt(m_SocketFD,
                        SOL_SOCKET,
                        SO_RCVBUF,
                        (SET_SOCKOPT_TYPE) &rx_buffer_size,
                        sizeof(rx_buffer_size)) == -1)
                throw std::runtime_error(
                        "PositionListener::ListenLoop()::setsockopt::rcvbuf");
 
            // construct a datagram address structure
            struct sockaddr_in dg_addr;
            memset(&dg_addr, 0, sizeof(dg_addr));
            dg_addr.sin_family = AF_INET;
            // listen on any address
            dg_addr.sin_addr.s_addr = htonl(INADDR_ANY);
            dg_addr.sin_port = htons(m_Port);
 
            if (bind(m_SocketFD,
                        (struct sockaddr *) &dg_addr,
                        sizeof(dg_addr)) == -1)
                throw std::runtime_error(
                        "PositionListener::ListenLoop()::bind");
 
            // reserve receive buffer, make it large enough (2 * serialized size
            // for now)
            PositionData pd;
            std::vector<char> incoming_buffer(pd.size() * 2);
            while (!StopRequested()) {
                if (ReadyToRead()) {
                    if (p_Owner->debugLevel() >= 3)
                        LOG_MSG_INFO("Trying to read data from peer");
                    int num_bytes_read = recvfrom(m_SocketFD,
                            (char *) incoming_buffer.data(),
                            (int) incoming_buffer.size(),
                            0,
                            0,
                            0);
                    if (num_bytes_read < 0) {
                        LOG_MSG_ERROR("Error reading from UDP port: "
                                << num_bytes_read);
                        continue;
                    }
                    if (p_Owner->debugLevel() >= 3)
                        LOG_MSG_INFO("Received " << num_bytes_read
                                                 << " bytes from peer");
                    try {
                        pd.deserialize(
                                incoming_buffer.data(), incoming_buffer.size());
                        p_Owner->setPosition(pd);
                    } catch (const std::exception &e) {
                        LOG_MSG_ERROR(
                                "Caught exception while reading position data: "
                                << e.what() << " - " << std::strerror(errno));
                    }
                }
            }
            if (p_Owner->debugLevel() > 0)
                LOG_MSG_INFO(
                        "Stopping position data listener on port " << m_Port);
        } catch (const std::exception &e) {
            LOG_MSG_ERROR("Caught exception in listening thread: "
                    << e.what() << " - " << std::strerror(errno));
        }
    }
 
protected:
    bool
    ReadyToRead()
    {
        int nfds;
        fd_set fdset;
 
        FD_ZERO(&fdset);
        FD_SET(m_SocketFD, &fdset);
 
        nfds = (int) m_SocketFD;
        int ret = select(nfds + 1, &fdset, NULL, NULL, &m_ReadTimeout);
        if (ret == SOCKET_ERROR) {
            LOG_MSG_ERROR("Error on select: " << ret);
        }
        return ret == 1;
    }
    SOCKET_TYPE m_SocketFD{0};
    timeval m_ReadTimeout;
    uint16_t m_Port;
    Owner *p_Owner;
};
 
#endif // _POSITION_LISTENER_H_