Fixed auto reconnection to the RTDE server. (#384)

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

@@ -366,13 +366,9 @@ class Pipeline
    */
   void stop()
   {
-    if (!running_)
-      return;
-
     URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
 
     running_ = false;
-
     producer_.stopProducer();
     if (pThread_.joinable())
     {

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_reconnect_cb_;
 
   bool running_;
 
@@ -124,9 +125,21 @@ class URProducer : public IProducer<T>
       if (!running_)
         return true;
 
+      if (stream_.getState() == SocketState::Connected)
+      {
+        continue;
+      }
+
       if (stream_.closed())
         return false;
 
+      if (on_reconnect_cb_)
+      {
+        URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer");
+        on_reconnect_cb_();
+        return false;
+      }
+
       URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
       std::this_thread::sleep_for(timeout_);
 
@@ -140,6 +153,18 @@ class URProducer : public IProducer<T>
 
     return false;
   }
+
+  /*!
+   * \brief Sets the reconnection callback. Use this to configure a reconnection callback instead of connecting directly
+   * to the stream again. This is needed for RTDE as it requires setting up the communication again upon reconnection it
+   * is not enough to just reconnect to the stream.
+   *
+   * \param on_reconnect_cb Callback to be invoked when connection is lost to the stream.
+   */
+  void setReconnectionCallback(std::function<void()> on_reconnect_cb)
+  {
+    on_reconnect_cb_ = on_reconnect_cb;
+  }
 };
 }  // namespace comm
 }  // namespace urcl

include/ur_client_library/rtde/rtde_client.h

@@ -231,6 +231,10 @@ class RTDEClient
   comm::INotifier notifier_;
   std::unique_ptr<comm::Pipeline<RTDEPackage>> pipeline_;
   RTDEWriter writer_;
+  std::atomic<bool> reconnecting_;
+  std::atomic<bool> stop_reconnection_;
+  std::mutex reconnect_mutex_;
+  std::thread reconnecting_thread_;
 
   VersionInformation urcontrol_version_;
 
@@ -249,12 +253,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,
+  bool setupCommunication(const size_t max_num_tries = 0,
                           const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
-  bool negotiateProtocolVersion(const uint16_t protocol_version);
-  void queryURControlVersion();
-  void setupOutputs(const uint16_t protocol_version);
-  void setupInputs();
+  uint16_t negotiateProtocolVersion();
+  bool queryURControlVersion();
+  void setTargetFrequency();
+  bool setupOutputs(const uint16_t protocol_version);
+  bool setupInputs();
   void disconnect();
 
   /*!
@@ -288,6 +293,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();
   }
 
   /*!
@@ -88,6 +84,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.
    *