Allow resetting the RTDE client

Author: Felix Exner

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,10 +487,42 @@ 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);
 
-  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_;
@@ -502,7 +534,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));
 
@@ -593,4 +586,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