[RTDE client] Test reconnect

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