[RTDE client] Test reconnect (UniversalRobots#433)

This adds an integration test for reconnecting to the RTDE server.
For this, a fake RTDEServer is added that can be stopped and restarted
inside the tests easier than managing a URSim instance.

The fake server doesn't have full functionality which is why it lives
inside the test repository and is not part of the client library shared
library target.

I've added a refactoring to merge two existing `splitStrings` functions
along the way before adding a third one.

Author: urfeex

include/ur_client_library/helpers.h

@@ -99,5 +99,15 @@ void waitFor(std::function<bool()> condition, const std::chrono::milliseconds ti
  */
 bool parseBoolean(const std::string& str);
 
+/*!
+ * \brief Splits a at each delimiter found
+ *
+ * \param string_to_split String containing the delimiter and other characters
+ * \param delimiter Chars at which the string should be split
+ *
+ * \returns A vector of characters that were between the delimiters
+ */
+std::vector<std::string> splitString(const std::string& string_to_split, const std::string& delimiter = ",");
+
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED

include/ur_client_library/rtde/rtde_client.h

@@ -84,7 +84,8 @@ enum class ClientState
   INITIALIZING = 1,
   INITIALIZED = 2,
   RUNNING = 3,
-  PAUSED = 4
+  PAUSED = 4,
+  CONNECTION_LOST = 5
 };
 
 /*!
@@ -105,11 +106,12 @@ class RTDEClient
    * \param input_recipe_file Path to the file containing the input recipe
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
    * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * \param port Optionally specify a different port
    * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
              const std::string& input_recipe_file, double target_frequency = 0.0,
-             bool ignore_unavailable_outputs = false);
+             bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT);
 
   /*!
    * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
@@ -121,11 +123,12 @@ class RTDEClient
    * \param input_recipe Vector containing the input recipe
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
    * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * \param port Optionally specify a different port
    * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector<std::string>& output_recipe,
              const std::vector<std::string>& input_recipe, double target_frequency = 0.0,
-             bool ignore_unavailable_outputs = false);
+             bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT);
   ~RTDEClient();
   /*!
    * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
@@ -235,6 +238,11 @@ class RTDEClient
   // Reads output or input recipe from a file
   static std::vector<std::string> readRecipe(const std::string& recipe_file);
 
+  ClientState getClientState() const
+  {
+    return client_state_;
+  }
+
 private:
   comm::URStream<RTDEPackage> stream_;
   std::vector<std::string> output_recipe_;
@@ -295,21 +303,16 @@ class RTDEClient
   bool sendStart();
   bool sendPause();
 
-  /*!
-   * \brief Splits a variable_types string as reported from the robot into single variable type
-   * strings
-   *
-   * \param variable_types String as reported from the robot
-   *
-   * \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();
+
+  size_t max_connection_attempts_;
+  std::chrono::milliseconds reconnection_timeout_;
+  size_t max_initialization_attempts_;
+  std::chrono::milliseconds initialization_timeout_;
 };
 
 }  // namespace rtde_interface

include/ur_client_library/ur/version_information.h

@@ -79,7 +79,6 @@ class VersionInformation
   uint32_t build;   ///< Build number
 };
 
-std::vector<std::string> splitString(std::string input, const std::string& delimiter = ".");
 }  // namespace urcl
 
-#endif  // ifndef UR_CLIENT_LIBRARY_UR_VERSION_INFORMATION_H_INCLUDED
+#endif  // ifndef UR_CLIENT_LIBRARY_UR_VERSION_INFORMATION_H_INCLUDED

src/helpers.cpp

@@ -140,4 +140,22 @@ bool parseBoolean(const std::string& str)
     throw UrException(ss.str().c_str());
   }
 }
+
+std::vector<std::string> splitString(const std::string& input, const std::string& delimiter)
+{
+  std::vector<std::string> result;
+  size_t pos = 0;
+  size_t pos_end = pos;
+  std::string substring;
+  while ((pos_end = input.find(delimiter, pos)) != std::string::npos)
+  {
+    substring = input.substr(pos, pos_end - pos);
+    result.push_back(substring);
+    pos = pos_end + delimiter.length();
+  }
+  substring = input.substr(pos);
+  result.push_back(substring);
+  return result;
+}
+
 }  // namespace urcl

src/rtde/rtde_client.cpp

@@ -31,14 +31,16 @@
 #include "ur_client_library/log.h"
 #include <algorithm>
 #include <chrono>
+#include <string>
 
 namespace urcl
 {
 namespace rtde_interface
 {
 RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
-                       const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs)
-  : stream_(robot_ip, UR_RTDE_PORT)
+                       const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs,
+                       const uint32_t port)
+  : stream_(robot_ip, port)
   , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file)))
   , ignore_unavailable_outputs_(ignore_unavailable_outputs)
   , parser_(output_recipe_)
@@ -61,8 +63,8 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
 
 RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector<std::string>& output_recipe,
                        const std::vector<std::string>& input_recipe, double target_frequency,
-                       bool ignore_unavailable_outputs)
-  : stream_(robot_ip, UR_RTDE_PORT)
+                       bool ignore_unavailable_outputs, const uint32_t port)
+  : stream_(robot_ip, port)
   , output_recipe_(ensureTimestampIsPresent(output_recipe))
   , ignore_unavailable_outputs_(ignore_unavailable_outputs)
   , input_recipe_(input_recipe)
@@ -103,6 +105,11 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m
     return true;
   }
 
+  max_connection_attempts_ = max_connection_attempts;
+  reconnection_timeout_ = reconnection_timeout;
+  max_initialization_attempts_ = max_initialization_attempts;
+  initialization_timeout_ = initialization_timeout;
+
   prod_->setReconnectionCallback(nullptr);
 
   unsigned int attempts = 0;
@@ -132,24 +139,27 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m
 
 bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time)
 {
-  // The state initializing is used inside disconnect to stop the pipeline again.
-  client_state_ = ClientState::INITIALIZING;
-  // A running pipeline is needed inside setup.
+  client_state_ = ClientState::UNINITIALIZED;
   try
   {
     pipeline_->init(max_num_tries, reconnection_time);
   }
   catch (const UrException& exc)
   {
-    URCL_LOG_ERROR("Caught exception %s, while trying to initialize pipeline", exc.what());
+    URCL_LOG_ERROR("Caught exception '%s', while trying to initialize pipeline", exc.what());
     return false;
   }
+  // The state initializing is used inside disconnect to stop the pipeline again.
+  // A running pipeline is needed inside setup.
+  client_state_ = ClientState::INITIALIZING;
+
   pipeline_->run();
 
   uint16_t protocol_version = negotiateProtocolVersion();
   // Protocol version must be above zero
   if (protocol_version == 0)
   {
+    client_state_ = ClientState::UNINITIALIZED;
     return false;
   }
 
@@ -286,7 +296,8 @@ void RTDEClient::setTargetFrequency()
   else if (target_frequency_ <= 0.0 || target_frequency_ > max_frequency_)
   {
     // Target frequency outside valid range
-    throw UrException("Invalid target frequency of RTDE connection");
+    std::string error = "Invalid target frequency of RTDE connection: " + std::to_string(target_frequency_);
+    throw UrException(error.c_str());
   }
 }
 
@@ -348,7 +359,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version)
             dynamic_cast<rtde_interface::ControlPackageSetupOutputs*>(package.get()))
 
     {
-      std::vector<std::string> variable_types = splitVariableTypes(tmp_output->variable_types_);
+      std::vector<std::string> variable_types = splitString(tmp_output->variable_types_, ",");
       std::vector<std::string> available_variables;
       std::vector<std::string> unavailable_variables;
       assert(output_recipe_.size() == variable_types.size());
@@ -441,7 +452,7 @@ bool RTDEClient::setupInputs()
             dynamic_cast<rtde_interface::ControlPackageSetupInputs*>(package.get()))
 
     {
-      std::vector<std::string> variable_types = splitVariableTypes(tmp_input->variable_types_);
+      std::vector<std::string> variable_types = splitString(tmp_input->variable_types_, ",");
       assert(input_recipe_.size() == variable_types.size());
       for (std::size_t i = 0; i < variable_types.size(); ++i)
       {
@@ -745,36 +756,24 @@ RTDEWriter& RTDEClient::getWriter()
   return writer_;
 }
 
-std::vector<std::string> RTDEClient::splitVariableTypes(const std::string& variable_types) const
-{
-  std::vector<std::string> result;
-  std::stringstream ss(variable_types);
-  std::string substr = "";
-  while (getline(ss, substr, ','))
-  {
-    result.push_back(substr);
-  }
-  return result;
-}
-
 void RTDEClient::reconnect()
 {
   URCL_LOG_INFO("Reconnecting to the RTDE interface");
   // Locking mutex to ensure that calling getDataPackage doesn't influence the communication needed for reconfiguring
   // the RTDE connection
   std::lock_guard<std::mutex> lock(reconnect_mutex_);
   ClientState cur_client_state = client_state_;
+  client_state_ = ClientState::CONNECTION_LOST;
   disconnect();
 
-  const size_t max_initialization_attempts = 3;
   size_t cur_initialization_attempt = 0;
   bool client_reconnected = false;
-  while (cur_initialization_attempt < max_initialization_attempts)
+  while (cur_initialization_attempt < max_initialization_attempts_)
   {
     bool is_communication_setup = false;
     try
     {
-      is_communication_setup = setupCommunication(1, std::chrono::milliseconds{ 10000 });
+      is_communication_setup = setupCommunication(max_connection_attempts_, reconnection_timeout_);
     }
     catch (const UrException& exc)
     {
@@ -797,24 +796,22 @@ void RTDEClient::reconnect()
       break;
     }
 
-    auto duration = std::chrono::seconds(1);
     if (stream_.getState() != comm::SocketState::Connected)
     {
       // We don't wanna count it as an initialization attempt if we cannot connect to the socket and we want to wait
       // longer before reconnecting.
-      duration = std::chrono::seconds(10);
-      URCL_LOG_ERROR("Failed to connect to the RTDE server, retrying in %i seconds", duration.count());
+      URCL_LOG_ERROR("Failed to connect to the RTDE server, retrying in %i seconds", reconnection_timeout_.count());
     }
     else
     {
-      URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %i second", duration.count());
+      URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %i second", initialization_timeout_.count());
       cur_initialization_attempt += 1;
     }
 
     disconnect();
 
     auto start_time = std::chrono::steady_clock::now();
-    while (std::chrono::steady_clock::now() - start_time < duration)
+    while (std::chrono::steady_clock::now() - start_time < initialization_timeout_)
     {
       std::this_thread::sleep_for(std::chrono::milliseconds(250));
       if (stop_reconnection_)
@@ -828,12 +825,14 @@ void RTDEClient::reconnect()
   if (client_reconnected == false)
   {
     URCL_LOG_ERROR("Failed to initialize RTDE client after %i attempts, unable to reconnect",
-                   max_initialization_attempts);
+                   max_initialization_attempts_);
     disconnect();
     reconnecting_ = false;
     return;
   }
 
+  URCL_LOG_INFO("Successfully reconnected to the RTDE interface, starting communication again");
+
   start();
   if (cur_client_state == ClientState::PAUSED)
   {

src/ur/version_information.cpp

@@ -27,24 +27,11 @@
 //----------------------------------------------------------------------
 
 #include <ur_client_library/exceptions.h>
+#include <ur_client_library/helpers.h>
 #include <ur_client_library/ur/version_information.h>
 
 namespace urcl
 {
-std::vector<std::string> splitString(std::string input, const std::string& delimiter)
-{
-  std::vector<std::string> result;
-  size_t pos = 0;
-  std::string substring;
-  while ((pos = input.find(delimiter)) != std::string::npos)
-  {
-    substring = input.substr(0, pos);
-    result.push_back(substring);
-    input.erase(0, pos + delimiter.length());
-  }
-  result.push_back(input);
-  return result;
-}
 
 VersionInformation::VersionInformation()
 {
@@ -58,7 +45,7 @@ VersionInformation::VersionInformation()
 
 VersionInformation VersionInformation::fromString(const std::string& str)
 {
-  auto components = splitString(str);
+  auto components = splitString(str, ".");
   VersionInformation info;
   if (components.size() >= 2)
   {
@@ -149,4 +136,4 @@ bool operator>=(const VersionInformation& v1, const VersionInformation& v2)
 {
   return !(v1 < v2);
 }
-}  // namespace urcl
+}  // namespace urcl

tests/CMakeLists.txt

@@ -14,6 +14,9 @@ FetchContent_Declare(
 set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
 FetchContent_MakeAvailable(googletest)
 
+add_executable(fake_rtde_server fake_rtde_server.cpp fake_rtde_server_main.cpp)
+target_link_libraries(fake_rtde_server PRIVATE ur_client_library::urcl)
+
 include(GoogleTest)
 
 option(INTEGRATION_TESTS "Build the integration tests that require a running robot / URSim" OFF)
@@ -25,7 +28,7 @@ if (INTEGRATION_TESTS)
   find_package(Python3 COMPONENTS Interpreter REQUIRED)
 
   add_custom_target(generate_outputs ALL COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/resources/generate_rtde_outputs.py)
-  add_executable(rtde_tests test_rtde_client.cpp)
+  add_executable(rtde_tests test_rtde_client.cpp fake_rtde_server.cpp)
   add_dependencies(rtde_tests generate_outputs)
   target_link_libraries(rtde_tests PRIVATE ur_client_library::urcl GTest::gtest_main)
   gtest_add_tests(TARGET      rtde_tests