Fixed auto reconnection to the RTDE server.

When reconnecting to RTDE it requires you to set the input and output recipes again, this is now done properly.

Author: urmahp

include/ur_client_library/comm/pipeline.h

@@ -299,6 +299,7 @@ class Pipeline
     , queue_{ 32 }
     , running_{ false }
     , producer_fifo_scheduling_(producer_fifo_scheduling)
+    , threads_stopped_(true)
   {
   }
   /*!
@@ -318,6 +319,7 @@ class Pipeline
     , queue_{ 32 }
     , running_{ false }
     , producer_fifo_scheduling_(producer_fifo_scheduling)
+    , threads_stopped_(true)
   {
   }
 
@@ -354,6 +356,7 @@ class Pipeline
       return;
 
     running_ = true;
+    threads_stopped_ = false;
     producer_.startProducer();
     pThread_ = std::thread(&Pipeline::runProducer, this);
     if (consumer_ != nullptr)
@@ -366,13 +369,13 @@ class Pipeline
    */
   void stop()
   {
-    if (!running_)
+    if (threads_stopped_)
       return;
 
     URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
 
     running_ = false;
-
+    threads_stopped_ = true;
     producer_.stopProducer();
     if (pThread_.joinable())
     {
@@ -417,6 +420,7 @@ class Pipeline
   std::atomic<bool> running_;
   std::thread pThread_, cThread_;
   bool producer_fifo_scheduling_;
+  bool threads_stopped_;
 
   void runProducer()
   {

include/ur_client_library/comm/producer.h

@@ -43,6 +43,7 @@ class URProducer : public IProducer<T>
   URStream<T>& stream_;
   Parser<T>& parser_;
   std::chrono::seconds timeout_;
+  std::function<void()> on_rtde_reconnect_cb_;
 
   bool running_;
 
@@ -124,9 +125,28 @@ class URProducer : public IProducer<T>
       if (!running_)
         return true;
 
+      if (stream_.getState() == SocketState::Connected)
+      {
+        continue;
+      }
+
       if (stream_.closed())
         return false;
 
+      if (stream_.getStreamType() == URStreamType::RTDE)
+      {
+        if (on_rtde_reconnect_cb_)
+        {
+          URCL_LOG_WARN("Failed to read from RTDE stream, invoking on reconnect callback and stopping the producer");
+          on_rtde_reconnect_cb_();
+        }
+        else
+        {
+          URCL_LOG_ERROR("Failed to read from RTDE stream without a reconnect handler stopping the producer");
+        }
+        return false;
+      }
+
       URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
 
@@ -140,6 +160,17 @@ class URProducer : public IProducer<T>
 
     return false;
   }
+
+  /*!
+   * \brief Sets the RTDE reconnection callback. RTDE requires setting up the communication again upon reconnection
+   * it is not enough to just reconnect to the stream.
+   *
+   * \param on_rtde_reconnect_cb Callback to be invoked when connection is lost to the RTDE stream.
+   */
+  void setRTDEReconnectionCallback(std::function<void()> on_rtde_reconnect_cb)
+  {
+    on_rtde_reconnect_cb_ = on_rtde_reconnect_cb;
+  }
 };
 }  // namespace comm
 }  // namespace urcl

include/ur_client_library/comm/stream.h

@@ -30,6 +30,18 @@ namespace urcl
 {
 namespace comm
 {
+
+/*!
+ * \brief Different types of UR streams
+ */
+enum class URStreamType
+{
+  Primary = 30001,    ///< Stream connected to the primary interface
+  Secondary = 30002,  ///< Stream connected to the secondary interface
+  RTDE = 30004,       ///< Stream connected to the RTDE interface
+  UNKNOWN = -1,       ///< Stream type is fetched from the port, this is to handle unknown ports
+};
+
 /*!
  * \brief The stream is an abstraction of the TCPSocket that offers reading a full UR data package
  * out of the socket. This means, it has to have some knowledge about the package structure to
@@ -117,6 +129,26 @@ class URStream : public TCPSocket
     return host_;
   }
 
+  /*!
+   * \brief Get the stream type
+   *
+   * \returns The stream type
+   */
+  URStreamType getStreamType()
+  {
+    switch (port_)
+    {
+      case static_cast<int>(URStreamType::Primary):
+        return URStreamType::Primary;
+      case static_cast<int>(URStreamType::Secondary):
+        return URStreamType::Secondary;
+      case static_cast<int>(URStreamType::RTDE):
+        return URStreamType::RTDE;
+      default:
+        return URStreamType::UNKNOWN;
+    }
+  }
+
 private:
   std::string host_;
   int port_;

include/ur_client_library/rtde/rtde_client.h

@@ -231,6 +231,9 @@ class RTDEClient
   comm::INotifier notifier_;
   std::unique_ptr<comm::Pipeline<RTDEPackage>> pipeline_;
   RTDEWriter writer_;
+  bool reconnecting_;
+  std::mutex reconnect_mutex_;
+  std::thread reconnecting_thread_;
 
   VersionInformation urcontrol_version_;
 
@@ -249,12 +252,13 @@ class RTDEClient
   // the robot is booted.
   std::vector<std::string> ensureTimestampIsPresent(const std::vector<std::string>& output_recipe) const;
 
-  void setupCommunication(const size_t max_num_tries = 0,
-                          const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
+  bool setupCommunication();
+  uint16_t setProtocolVersion();
   bool negotiateProtocolVersion(const uint16_t protocol_version);
-  void queryURControlVersion();
-  void setupOutputs(const uint16_t protocol_version);
-  void setupInputs();
+  bool queryURControlVersion();
+  bool setTargetFrequency();
+  bool setupOutputs(const uint16_t protocol_version);
+  bool setupInputs();
   void disconnect();
 
   /*!
@@ -288,6 +292,12 @@ class RTDEClient
    * \returns A vector of variable variable_names
    */
   std::vector<std::string> splitVariableTypes(const std::string& variable_types) const;
+
+  /*!
+   * \brief Reconnects to the RTDE interface and set the input and output recipes again.
+   */
+  void reconnect();
+  void reconnectCallback();
 };
 
 }  // namespace rtde_interface

include/ur_client_library/rtde/rtde_writer.h

@@ -61,11 +61,7 @@ class RTDEWriter
 
   ~RTDEWriter()
   {
-    running_ = false;
-    if (writer_thread_.joinable())
-    {
-      writer_thread_.join();
-    }
+    stop();
   }
   /*!
    * \brief Starts the writer thread, which periodically clears the queue to write packages to the
@@ -79,6 +75,11 @@ class RTDEWriter
    */
   void run();
 
+  /*!
+   * \brief Stops the writer thread loop.
+   */
+  void stop();
+
   /*!
    * \brief Creates a package to request setting a new value for the speed slider.
    *