From e1e71b5cb4b7b14885517999645c54f60df07628 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Tue, 11 Jun 2019 17:55:03 +0200
Subject: [PATCH] setup timeout for tcp sockets

If we want to be able to stop the producer, we'll have to make sure,
it is not hanging inside the recv function because no new packages came in.
---
 .../include/ur_rtde_driver/comm/producer.h       |  4 ++++
 .../include/ur_rtde_driver/comm/tcp_socket.h     |  9 +++++++++
 ur_rtde_driver/src/comm/tcp_socket.cpp           | 16 ++++++++++++++++
 3 files changed, 29 insertions(+)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/producer.h b/ur_rtde_driver/include/ur_rtde_driver/comm/producer.h
index e309697..58a20ee 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/comm/producer.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/comm/producer.h
@@ -44,6 +44,10 @@ public:
 
   void setupProducer()
   {
+    timeval tv;
+    tv.tv_sec = 1;
+    tv.tv_usec = 0;
+    stream_.setReceiveTimeout(tv);
     stream_.connect();
   }
   void teardownProducer()
diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h b/ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h
index a9c51a6..54400d7 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/comm/tcp_socket.h
@@ -23,6 +23,7 @@
 #include <atomic>
 #include <mutex>
 #include <string>
+#include <memory>
 
 namespace ur_driver
 {
@@ -44,6 +45,7 @@ class TCPSocket
 private:
   std::atomic<int> socket_fd_;
   std::atomic<SocketState> state_;
+  std::unique_ptr<timeval> recv_timeout_;
 
 protected:
   virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
@@ -111,6 +113,13 @@ public:
    * \brief Closes the connection to the socket.
    */
   void close();
+
+  /*!
+   * \brief Setup Receive timeout used for this socket.
+   *
+   * \param timeout Timeout used for setting things up
+   */
+  void setReceiveTimeout(const timeval& timeout);
 };
 }  // namespace comm
 }  // namespace ur_driver
diff --git a/ur_rtde_driver/src/comm/tcp_socket.cpp b/ur_rtde_driver/src/comm/tcp_socket.cpp
index 76ec7c6..a8337e3 100644
--- a/ur_rtde_driver/src/comm/tcp_socket.cpp
+++ b/ur_rtde_driver/src/comm/tcp_socket.cpp
@@ -44,6 +44,11 @@ void TCPSocket::setOptions(int socket_fd)
   int flag = 1;
   setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(int));
   setsockopt(socket_fd, IPPROTO_TCP, TCP_QUICKACK, &flag, sizeof(int));
+
+  if (recv_timeout_ != nullptr)
+  {
+    setsockopt(socket_fd, SOL_SOCKET, SO_RCVTIMEO, recv_timeout_.get(), sizeof(timeval));
+  }
 }
 
 bool TCPSocket::setup(std::string& host, int port)
@@ -188,5 +193,16 @@ bool TCPSocket::write(const uint8_t* buf, const size_t buf_len, size_t& written)
 
   return true;
 }
+
+void TCPSocket::setReceiveTimeout(const timeval& timeout)
+{
+  recv_timeout_.reset(new timeval(timeout));
+
+  if (state_ == SocketState::Connected)
+  {
+    setOptions(socket_fd_);
+  }
+}
+
 }  // namespace comm
 }  // namespace ur_driver