Allow resetting the RTDE client (#218)

With some control modes such as the trajectory passthrough controller it
is actually possible to run the communication at much lower frequencies.

However, so far we always initialize the `RTDEClient` with the maximum
frequency that is possible on the respective model.

This adds a function to `UrDriver` to reset the `RTDEClient` with
another communication frequency.

## ToDo

- [x] Implementation
- [x] Write tests
- [x] Write documentation

Author: Felix Exner (fexner)

doc/architecture.rst

@@ -63,6 +63,11 @@ sure to
 * run it from the package's main folder, as for simplicity reasons it doesn't use any sophisticated
   method to locate the required files.
 
+.. note::
+   The ``URDriver`` class creates a ``RTDEClient`` during initialization using the provided
+   recipes and utilizing the robot model's maximum frequency. If you would like to use a different
+   frequency, please use the ``resetRTDEClient()`` method after the ``UrDriver`` object has been
+   created.
 
 RTDEWriter
 ^^^^^^^^^^

doc/conf.py

@@ -27,7 +27,7 @@
 # -- Project information -----------------------------------------------------
 
 project = "ur_client_library"
-copyright = "2022, Felix Exner"
+copyright = "2022, Universal Robots A/S"
 author = "Felix Exner"
 
 # The short X.Y version

include/ur_client_library/ur/ur_driver.h

@@ -195,7 +195,7 @@ class UrDriver
 
   uint32_t getControlFrequency() const
   {
-    return rtde_frequency_;
+    return rtde_client_->getTargetFrequency();
   }
 
   /*!
@@ -487,6 +487,36 @@ class UrDriver
     script_command_interface_->setToolContactResultCallback(tool_contact_result_cb);
   }
 
+  /**
+   * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication
+   * with the maximum available frequency, this enables users to start RTDE communication with a
+   * custom rate.
+   *
+   * Note: After calling this function startRTDECommunication() has to be called again to actually
+   * start RTDE communication.
+   *
+   * \param output_recipe Vector containing the output recipe
+   * \param input_recipe Vector containing the input recipe
+   * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
+   */
+  void resetRTDEClient(const std::vector<std::string>& output_recipe, const std::vector<std::string>& input_recipe,
+                       double target_frequency = 0.0);
+
+  /**
+   * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication
+   * with the maximum available frequency, this enables users to start RTDE communication with a
+   * custom rate.
+   *
+   * Note: After calling this function startRTDECommunication() has to be called again to actually
+   * start RTDE communication.
+   *
+   * \param output_recipe_filename Filename where the output recipe is stored in.
+   * \param input_recipe_filename Filename where the input recipe is stored in.
+   * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
+   */
+  void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
+                       double target_frequency = 0.0);
+
 private:
   static std::string readScriptFile(const std::string& filename);
   /*!
@@ -498,7 +528,9 @@ class UrDriver
    */
   bool reconnectSecondaryStream();
 
-  int rtde_frequency_;
+  void initRTDE();
+  void setupReverseInterface(const uint32_t reverse_port);
+
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
   std::unique_ptr<control::ReverseInterface> reverse_interface_;
@@ -510,7 +542,6 @@ class UrDriver
 
   uint32_t servoj_gain_;
   double servoj_lookahead_time_;
-  std::chrono::milliseconds step_time_;
 
   std::function<void(bool)> handle_program_state_;
 

src/ur/ur_driver.cpp

@@ -61,7 +61,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
                          const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling)
   : servoj_gain_(servoj_gain)
   , servoj_lookahead_time_(servoj_lookahead_time)
-  , step_time_(std::chrono::milliseconds(8))
   , handle_program_state_(handle_program_state)
   , robot_ip_(robot_ip)
 {
@@ -78,13 +77,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
   non_blocking_read_ = non_blocking_read;
   get_packet_timeout_ = non_blocking_read_ ? 0 : 100;
 
-  if (!rtde_client_->init())
-  {
-    throw UrException("Initialization of RTDE client went wrong.");
-  }
-
-  rtde_frequency_ = rtde_client_->getMaxFrequency();
-  step_time_ = std::chrono::milliseconds(1000 / rtde_frequency_);
+  initRTDE();
+  setupReverseInterface(reverse_port);
 
   // Figure out the ip automatically if the user didn't provide it
   std::string local_ip = reverse_ip.empty() ? rtde_client_->getIP() : reverse_ip;
@@ -210,7 +204,6 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
     URCL_LOG_DEBUG("Created script sender");
   }
 
-  reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state, step_time_));
   trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port));
   script_command_interface_.reset(new control::ScriptCommandInterface(script_command_port));
 
@@ -621,4 +614,35 @@ void UrDriver::setKeepaliveCount(const uint32_t count)
   reverse_interface_->setKeepaliveCount(count);
 }
 
+void UrDriver::resetRTDEClient(const std::vector<std::string>& output_recipe,
+                               const std::vector<std::string>& input_recipe, double target_frequency)
+{
+  rtde_client_.reset(
+      new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe, target_frequency));
+  initRTDE();
+}
+
+void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
+                               double target_frequency)
+{
+  rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_filename, input_recipe_filename,
+                                                    target_frequency));
+  initRTDE();
+}
+
+void UrDriver::initRTDE()
+{
+  if (!rtde_client_->init())
+  {
+    throw UrException("Initialization of RTDE client went wrong.");
+  }
+}
+
+void UrDriver::setupReverseInterface(const uint32_t reverse_port)
+{
+  auto rtde_frequency = rtde_client_->getTargetFrequency();
+  auto step_time = std::chrono::milliseconds(static_cast<int>(1000 / rtde_frequency));
+  reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time));
+}
+
 }  // namespace urcl

tests/test_ur_driver.cpp

@@ -372,6 +372,13 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure)
   EXPECT_TRUE(g_ur_driver_->sendRobotProgram());
 }
 
+TEST_F(UrDriverTest, reset_rtde_client)
+{
+  double target_frequency = 50;
+  g_ur_driver_->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency);
+  ASSERT_EQ(g_ur_driver_->getControlFrequency(), target_frequency);
+}
+
 // TODO we should add more tests for the UrDriver class.
 
 int main(int argc, char* argv[])