Merge pull request #12 from Brazilian-Institute-of-Robotics/bidirecti…

…onal_udp

Bidirectional udp

src/Driver.cpp

@@ -194,6 +194,16 @@ void Driver::openURI(std::string const& uri)
     { // UDP udp://hostname:remoteport
         if (marker == string::npos)
             throw std::runtime_error("missing port specification in udp:// URI");
+
+        string::size_type remote_port_marker = device.find_last_of(":");
+        if (remote_port_marker != string::npos)
+        {
+            int remote_port = boost::lexical_cast<int>(device.substr(remote_port_marker + 1));
+            device = device.substr(0, remote_port_marker);
+
+            return openUDPBidirectional(device, remote_port, additional_info);
+        }
+
         return openUDP(device, additional_info);
     }
     else if (mode_idx == 3)
@@ -218,7 +228,7 @@ bool Driver::openInet(const char *hostname, int port)
     return true;
 }
 
-void Driver::openIPServer(int port, addrinfo const& hints)
+static int createIPServerSocket(int port, addrinfo const& hints)
 {
     struct addrinfo *result;
     string port_as_string = boost::lexical_cast<string>(port);
@@ -244,16 +254,15 @@ void Driver::openIPServer(int port, addrinfo const& hints)
     if (rp == NULL)
         throw UnixError("cannot open server socket on port " + port_as_string);
 
-    setMainStream(new UDPServerStream(sfd,true));
+    return sfd;
 }
 
-void Driver::openIPClient(std::string const& hostname, int port, addrinfo const& hints)
+static int createIPClientSocket(const char *hostname, const char *port, addrinfo const& hints, struct sockaddr *addr, size_t *addr_len)
 {
     struct addrinfo *result;
-    string port_as_string = boost::lexical_cast<string>(port);
-    int ret = getaddrinfo(hostname.c_str(), port_as_string.c_str(), &hints, &result);
+    int ret = getaddrinfo(hostname, port, &hints, &result);
     if (ret != 0)
-        throw UnixError("cannot resolve host/port " + hostname + "/" + port_as_string);
+        throw UnixError("cannot resolve client port " + string(port));
 
     int sfd = -1;
     struct addrinfo *rp;
@@ -268,11 +277,24 @@ void Driver::openIPClient(std::string const& hostname, int port, addrinfo const&
 
         ::close(sfd);
     }
-    freeaddrinfo(result);
 
     if (rp == NULL)
-        throw UnixError("cannot open client socket to " + hostname + " port=" + boost::lexical_cast<string>(port));
+    {
+        freeaddrinfo(result);
+        throw UnixError("cannot open client socket on port " + string(port));
+    }
+
+    if (addr != NULL) *addr = *(rp->ai_addr);
+    if (addr_len != NULL) *addr_len = rp->ai_addrlen;
 
+    freeaddrinfo(result);
+
+    return sfd;
+}
+
+void Driver::openIPClient(std::string const& hostname, int port, addrinfo const& hints)
+{
+    int sfd = createIPClientSocket(hostname.c_str(), boost::lexical_cast<string>(port).c_str(), hints, NULL, NULL);
     setFileDescriptor(sfd);
 }
 
@@ -304,7 +326,9 @@ void Driver::openUDP(std::string const& hostname, int port)
         hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
         hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
         hints.ai_flags = AI_PASSIVE;    /* For wildcard IP address */
-        openIPServer(port, hints);
+
+        int sfd = createIPServerSocket(port, hints);
+        setMainStream(new UDPServerStream(sfd,true));
     }
     else
     {
@@ -316,6 +340,28 @@ void Driver::openUDP(std::string const& hostname, int port)
     }
 }
 
+void Driver::openUDPBidirectional(std::string const& hostname, int out_port, int in_port)
+{
+    struct addrinfo in_hints;
+    memset(&in_hints, 0, sizeof(struct addrinfo));
+    in_hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
+    in_hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+    in_hints.ai_flags = AI_PASSIVE;    /* For wildcard IP address */
+
+    struct addrinfo out_hints;
+    memset(&out_hints, 0, sizeof(struct addrinfo));
+    out_hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
+    out_hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+
+    struct sockaddr peer;
+    size_t peer_len;
+    int peerfd = createIPClientSocket(hostname.c_str(), boost::lexical_cast<string>(out_port).c_str(), out_hints, &peer, &peer_len);
+    ::close(peerfd);    
+
+    int sfd = createIPServerSocket(in_port, in_hints);
+    setMainStream(new UDPServerStream(sfd, true, &peer, &peer_len));
+}
+
 int Driver::openSerialIO(std::string const& port, int baud_rate)
 {
     int fd = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_SYNC | O_NONBLOCK );

src/Driver.hpp

@@ -155,7 +155,6 @@ class Driver
 
     mutable Status m_stats;
 
-    void openIPServer(int port, addrinfo const& hints);
     void openIPClient(std::string const& hostname, int port, addrinfo const& hints);
 
 public:
@@ -227,7 +226,7 @@ class Driver
      *
      * * serial://path/to/device:baudrate
      * * tcp://hostname:port
-     * * udp://hostname:port
+     * * udp://hostname:remote_port[:local_port]
      * * udpserver://port
      */
     virtual void openURI(std::string const& uri);
@@ -254,6 +253,16 @@ class Driver
     */
     void openUDP(std::string const& hostname, int remote_port);
 
+    /**
+    * Opens a UDP connection
+    *
+    * All parameters are required. The driver will be available to
+    * write data to a specified host and output port. Data will be read
+    * from the input port.
+    */
+    void openUDPBidirectional(std::string const& hostname, int out_port, int in_port);
+
+
     /** Opens a serial port and sets it up to a sane configuration.  Use
      * then setSerialBaudrate() to change the actual baudrate of the
      * connection on this side.

src/IOStream.cpp

@@ -99,11 +99,26 @@ UDPServerStream::UDPServerStream(int fd, bool auto_close)
   : FDStream(fd,auto_close)
 {
   m_s_len = sizeof(m_si_other);
+  m_si_other_dynamic = true;
 }
-
+
+UDPServerStream::UDPServerStream(int fd, bool auto_close, struct sockaddr *si_other, size_t *s_len)
+  : FDStream(fd,auto_close)
+{
+    m_si_other = *si_other;
+    m_s_len = *s_len;
+    m_si_other_dynamic = false;
+}
+
 size_t UDPServerStream::read(uint8_t* buffer, size_t buffer_size)
 {
-  ssize_t ret = recvfrom(m_fd, buffer, buffer_size, 0, &m_si_other, &m_s_len);
+  ssize_t ret;
+
+  if (m_si_other_dynamic)
+    ret = recvfrom(m_fd, buffer, buffer_size, 0, &m_si_other, &m_s_len);
+  else
+    ret = recvfrom(m_fd, buffer, buffer_size, 0, NULL, NULL);
+
   if (ret >= 0){
      return ret;
   }

src/IOStream.hpp

@@ -65,10 +65,12 @@ namespace iodrivers_base
     {
     public: 
       UDPServerStream(int fd, bool auto_close);
+      UDPServerStream(int fd, bool auto_close, struct sockaddr *si_other, size_t *s_len);
       virtual size_t read(uint8_t* buffer, size_t buffer_size);
       virtual size_t write(uint8_t const* buffer, size_t buffer_size);    
     protected:
       struct sockaddr m_si_other;
+      bool m_si_other_dynamic;
       unsigned int m_s_len;
     };
 }

test/test.cpp

@@ -4,7 +4,7 @@
 #define BOOST_AUTO_TEST_MAIN
 #include <boost/test/auto_unit_test.hpp>
 #include <boost/test/unit_test.hpp>
-
+#include <boost/thread.hpp>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -310,3 +310,62 @@ BOOST_AUTO_TEST_CASE(test_hasPacket_returns_false_on_internal_buffer_with_garbag
     BOOST_REQUIRE_EQUAL(4, test.readPacket(buffer, 100, 10, 1));
     BOOST_REQUIRE(!test.hasPacket());
 }
+
+BOOST_AUTO_TEST_CASE(test_open_bidirectional_udp)
+{
+    DriverTest test;
+
+    BOOST_REQUIRE_NO_THROW(test.openURI("udp://127.0.0.1:1111:2222"));
+    test.close();
+}
+
+void recv_test()
+{
+    DriverTest test;
+    uint8_t msg[4] = { 0, 'a', 'b', 0 };
+
+    test.openURI("udp://127.0.0.1:2125");
+
+    test.writePacket(msg, 4);
+    test.close();
+}
+
+BOOST_AUTO_TEST_CASE(test_recv_from_bidirectional_udp)
+{
+    DriverTest test;
+    uint8_t buffer[100];
+    int count;
+    uint8_t msg[4] = { 0, 'a', 'b', 0 };
+
+    BOOST_REQUIRE_NO_THROW(test.openURI("udp://127.0.0.1:3135:2125"));
+
+    recv_test();
+
+    BOOST_REQUIRE_NO_THROW((count = test.readPacket(buffer, 100, 200)));
+    BOOST_REQUIRE_EQUAL(count, 4);
+    BOOST_REQUIRE_EQUAL(memcmp(buffer, msg, count), 0);
+
+    test.close();
+}
+
+BOOST_AUTO_TEST_CASE(test_send_from_bidirectional_udp)
+{
+    DriverTest test;
+    DriverTest peer;
+
+    uint8_t buffer[100];
+    int count = 0;
+    uint8_t msg[4] = { 0, 'a', 'b', 0 };
+
+    BOOST_REQUIRE_NO_THROW(peer.openURI("udpserver://4145"));
+    BOOST_REQUIRE_NO_THROW(test.openURI("udp://127.0.0.1:4145:5155"));
+
+    BOOST_REQUIRE_NO_THROW(test.writePacket(msg, 4));
+    BOOST_REQUIRE_NO_THROW(count = peer.readPacket(buffer, 100, 500));
+
+    test.close();
+    peer.close();
+
+    BOOST_REQUIRE((count == 4) && (memcmp(buffer, msg, count) == 0));
+}
+