udpsocket.cpp 2.15 KB
Newer Older
Sydney Speckle's avatar
Sydney Speckle committed
1 2 3 4 5 6 7
/*
 * udpsocket.cpp
 *
 *  Created on: Feb 10, 2016
 *      Author: speckle_s
 */

8
#include "emros_dispatcher/udpsocket.h"
Sydney Speckle's avatar
Sydney Speckle committed
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97

UdpSocket::UdpSocket() :
    mPort(), mFd(), mSourceAddr(), mTargetaddr(), error(false), mTargetIP(), mSourceIP(), mTimeout()
{

}

UdpSocket::~UdpSocket()
{

  close_socket();
}

void UdpSocket::close_socket()
{
  close(mFd);
  ROS_INFO("close port %i", mPort);
}

int UdpSocket::open_socket(unsigned int port, const char* target_ip, const char* source_ip, int timeout)
{

  mPort = port;
  mSourceIP = source_ip;
  mTargetIP = target_ip;
  mTimeout = timeout;

  mSourceAddr.sin_addr.s_addr = inet_addr(mSourceIP);
  mSourceAddr.sin_family = AF_INET;
  mSourceAddr.sin_port = htons(mPort);

  mTargetaddr.sin_addr.s_addr = inet_addr(mTargetIP);
  mTargetaddr.sin_family = AF_INET;
  mTargetaddr.sin_port = htons(mPort);

  std::stringstream stream;
  /* create a UDP socket */
  if ((mFd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    stream << "cannot create socket\n";
    std::cout << (stream.str()) << std::endl;
    error = true;
  }

  /* bind the socket to any valid IP address and a specific port */

  if ((bind(mFd, (struct sockaddr *)&mSourceAddr, sizeof(mSourceAddr)) < 0) && error == false)
  {

    ROS_FATAL("bind failed %s at %i", mSourceIP, mPort);
    error = true;
  }
  else if (error == false)
  {
    ROS_INFO("success! binding %s port %i", mSourceIP, mPort);
  }
  if (error == true)
  {
    return -1;
  }
  else
  {
    return 0;
  }
}

int UdpSocket::udp_send(unsigned char* data, int length)
{
  socklen_t addrlen = sizeof(mTargetaddr); /* length of addresses */
  return sendto(mFd, data, length, 0, (struct sockaddr *)&mTargetaddr, addrlen);

}

void UdpSocket::listen_port(Ethprotocol::StreamBuffer& stream_buffer)
{
  std::stringstream stream;

  socklen_t addrlen = sizeof(mTargetaddr); /* length of addresses */

  fcntl(mFd, F_SETFL, O_NONBLOCK);
  timeval tv;
  tv.tv_sec = 0;
  tv.tv_usec = mTimeout;
  //setsockopt(mFd, SOL_SOCKET, SO_RCVTIMEO,&tv,sizeof(tv));

  stream_buffer.length = recvfrom(mFd, stream_buffer.data.begin(), stream_buffer.data.size(), 0,
                                  (struct sockaddr *)&mTargetaddr, &addrlen);
}