WIP: Implement ReverseInterface with new TCPServer

include/ur_client_library/comm/reverse_interface.h

@@ -28,10 +28,12 @@
 #ifndef UR_CLIENT_LIBRARY_REVERSE_INTERFACE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_REVERSE_INTERFACE_H_INCLUDED
 
-#include "ur_client_library/comm/server.h"
+#include "ur_client_library/comm/tcp_server.h"
 #include "ur_client_library/types.h"
+#include "ur_client_library/log.h"
 #include <cstring>
 #include <endian.h>
+#include <condition_variable>
 
 namespace urcl
 {
@@ -61,24 +63,26 @@ class ReverseInterface
    * \brief Creates a ReverseInterface object including a URServer.
    *
    * \param port Port the Server is started on
+   * \param handle_program_state Function handle to a callback on program state changes. For this to
+   * work, the URScript program will have to send keepalive signals to the \p reverse_port. I no
+   * keepalive signal can be read, program state will be false.
    */
-  ReverseInterface(uint32_t port) : server_(port)
+  ReverseInterface(uint32_t port, std::function<void(bool)> handle_program_state)
+    : client_fd_(-1), server_(port), handle_program_state_(handle_program_state)
   {
-    if (!server_.bind())
-    {
-      throw std::runtime_error("Could not bind to server");
-    }
-    if (!server_.accept())
-    {
-      throw std::runtime_error("Failed to accept robot connection");
-    }
+    handle_program_state_(false);
+    server_.setMessageCallback(
+        std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2));
+    server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1));
+    server_.setDisconnectCallback(std::bind(&ReverseInterface::disconnectionCallback, this, std::placeholders::_1));
+    server_.setMaxClientsAllowed(1);
+    server_.start();
   }
   /*!
    * \brief Disconnects possible clients so the reverse interface object can be safely destroyed.
    */
   ~ReverseInterface()
   {
-    server_.disconnectClient();
   }
 
   /*!
@@ -92,6 +96,10 @@ class ReverseInterface
    */
   bool write(const vector6d_t* positions, const ControlMode control_mode = ControlMode::MODE_IDLE)
   {
+    if (client_fd_ == -1)
+    {
+      return false;
+    }
     uint8_t buffer[sizeof(int32_t) * 8];
     uint8_t* b_pos = buffer;
 
@@ -118,7 +126,7 @@ class ReverseInterface
 
     size_t written;
 
-    return server_.write(buffer, sizeof(buffer), written);
+    return server_.write(client_fd_, buffer, sizeof(buffer), written);
   }
 
   /*!
@@ -128,23 +136,54 @@ class ReverseInterface
    */
   std::string readKeepalive()
   {
-    const size_t buf_len = 16;
-    char buffer[buf_len];
+    // If client has been disconnected / not connected yet.
+    if (client_fd_ == -1)
+    {
+      return "";
+    }
 
-    bool read_successful = server_.readLine(buffer, buf_len);
+    std::unique_lock<std::mutex> lk(keepalive_mutex_);
+    keepalive_cv_.wait(lk);
+    return keepalive_message_;
+  }
 
-    if (read_successful)
+private:
+  void connectionCallback(const int filedescriptor)
+  {
+    if (client_fd_ < 0)
     {
-      return std::string(buffer);
+      URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands.");
+      client_fd_ = filedescriptor;
+      handle_program_state_(true);
     }
     else
     {
-      return std::string("");
+      URCL_LOG_ERROR("Connection request to ReverseInterface received while connection already established. Only one "
+                     "connection is allowed at a time. Ignoring this request.");
     }
   }
 
-private:
-  URServer server_;
+  void disconnectionCallback(const int filedescriptor)
+  {
+    URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor);
+    client_fd_ = -1;
+    handle_program_state_(false);
+  }
+
+  void messageCallback(const int filedescriptor, char* buffer)
+  {
+    std::lock_guard<std::mutex> lk(keepalive_mutex_);
+    keepalive_message_ = std::string(buffer);
+    keepalive_cv_.notify_one();
+  }
+
+  int client_fd_;
+  TCPServer server_;
+
+  std::mutex keepalive_mutex_;
+  std::condition_variable keepalive_cv_;
+  std::string keepalive_message_;
+
   static const int32_t MULT_JOINTSTATE = 1000000;
 
   template <typename T>
@@ -154,6 +193,8 @@ class ReverseInterface
     std::memcpy(buffer, &val, s);
     return s;
   }
+
+  std::function<void(bool)> handle_program_state_;
 };
 
 }  // namespace comm

src/comm/tcp_server.cpp

@@ -167,7 +167,8 @@ void TCPServer::handleConnect()
   }
   else
   {
-    URCL_LOG_WARN("Connection attempt on port %d while maximum number of clients (%d) is already connected. Closing connection.",
+    URCL_LOG_WARN("Connection attempt on port %d while maximum number of clients (%d) is already connected. Closing "
+                  "connection.",
                   port_, max_clients_allowed_);
     close(client_fd);
   }

src/ur/ur_driver.cpp

@@ -56,8 +56,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
   : servoj_time_(0.008)
   , servoj_gain_(servoj_gain)
   , servoj_lookahead_time_(servoj_lookahead_time)
-  , reverse_interface_active_(false)
-  , reverse_port_(reverse_port)
   , handle_program_state_(handle_program_state)
   , robot_ip_(robot_ip)
 {
@@ -150,8 +148,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
     URCL_LOG_DEBUG("Created script sender");
   }
 
-  // reverse_port_ = reverse_port;
-  // watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this);
+  reverse_interface_.reset(new comm::ReverseInterface(reverse_port, handle_program_state));
 
   URCL_LOG_DEBUG("Initialization done");
 }
@@ -168,21 +165,13 @@ std::unique_ptr<rtde_interface::DataPackage> urcl::UrDriver::getDataPackage()
 
 bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode)
 {
-  if (reverse_interface_active_)
-  {
-    return reverse_interface_->write(&values, control_mode);
-  }
-  return false;
+  return reverse_interface_->write(&values, control_mode);
 }
 
 bool UrDriver::writeKeepalive()
 {
-  if (reverse_interface_active_)
-  {
-    vector6d_t* fake = nullptr;
-    return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE);
-  }
-  return false;
+  vector6d_t* fake = nullptr;
+  return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE);
 }
 
 void UrDriver::startRTDECommunication()
@@ -192,46 +181,8 @@ void UrDriver::startRTDECommunication()
 
 bool UrDriver::stopControl()
 {
-  if (reverse_interface_active_)
-  {
-    vector6d_t* fake = nullptr;
-    return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED);
-  }
-  return false;
-}
-
-void UrDriver::startWatchdog()
-{
-  handle_program_state_(false);
-  reverse_interface_.reset(new comm::ReverseInterface(reverse_port_));
-  reverse_interface_active_ = true;
-  URCL_LOG_DEBUG("Created reverse interface");
-
-  while (true)
-  {
-    URCL_LOG_INFO("Robot ready to receive control commands.");
-    handle_program_state_(true);
-    while (reverse_interface_active_ == true)
-    {
-      std::string keepalive = readKeepalive();
-
-      if (keepalive == std::string(""))
-      {
-        reverse_interface_active_ = false;
-      }
-    }
-
-    URCL_LOG_INFO("Connection to robot dropped, waiting for new connection.");
-    handle_program_state_(false);
-    // We explicitly call the destructor here, as unique_ptr.reset() creates a new object before
-    // replacing the pointer and destroying the old object. This will result in a resource conflict
-    // when trying to bind the socket.
-    // TODO: It would probably make sense to keep the same instance alive for the complete runtime
-    // instead of killing it all the time.
-    reverse_interface_->~ReverseInterface();
-    reverse_interface_.reset(new comm::ReverseInterface(reverse_port_));
-    reverse_interface_active_ = true;
-  }
+  vector6d_t* fake = nullptr;
+  return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED);
 }
 
 std::string UrDriver::readScriptFile(const std::string& filename)
@@ -243,14 +194,7 @@ std::string UrDriver::readScriptFile(const std::string& filename)
 }
 std::string UrDriver::readKeepalive()
 {
-  if (reverse_interface_active_)
-  {
-    return reverse_interface_->readKeepalive();
-  }
-  else
-  {
-    return std::string("");
-  }
+  return reverse_interface_->readKeepalive();
 }
 
 void UrDriver::checkCalibration(const std::string& checksum)