Added tests for the rtde interface clasess (#125)

* Added tests for the rtde interface clasess

This adds tests for
* control package pause
* control package start
* control package setup outputs
* control package setup inputs
* get urcontrol version
* request protocol version
* rtde writer

Added more tests to
* test_rtde_data_package
* test_rtde_parser
* test_rtde_client

Added functionality to rtde_client
* get_target_frequency function and warning when it is not possible to set target frequency
* Read timeout in isRobotBooted is now based on target frequency
* Added a wait on 5 seconds to receive answer from pause request. This will prevent the driver from crashing once in a while when pausing the rtde client

Added functionality for rtde_writer
* Added checks for values and pins before sending to the robot
* Removed unnecessary sleep in desctructor

Added pipeline changes, so that the test output is shown in the pipeline in case of failure

Co-authored-by: Felix Exner <[email protected]>

Author: urmahp

.github/workflows/ci.yml

@@ -35,7 +35,7 @@ jobs:
       - name: build
         run: cmake --build build --config Debug
       - name: test
-        run: cd build && make test
+        run: cd build && ctest --output-on-failure
       - name: install gcovr
         run: sudo apt-get install -y gcovr
       - name: gcovr

include/ur_client_library/rtde/rtde_client.h

@@ -122,7 +122,7 @@ class RTDEClient
   /*!
    * \brief Pauses RTDE data package communication
    *
-   * \returns Wheter the RTDE data package communication was paussed succesfully
+   * \returns Whether the RTDE data package communication was paused successfully
    */
   bool pause();
   /*!
@@ -135,15 +135,25 @@ class RTDEClient
   std::unique_ptr<rtde_interface::DataPackage> getDataPackage(std::chrono::milliseconds timeout);
 
   /*!
-   * \brief Getter for the frequency the robot will publish RTDE data packages with.
+   * \brief Getter for the maximum frequency the robot can publish RTDE data packages with.
    *
-   * \returns The used frequency
+   * \returns The maximum frequency
    */
   double getMaxFrequency() const
   {
     return max_frequency_;
   }
 
+  /*!
+   * \brief Getter for the target frequency that the robot will publish RTDE data packages with.
+   *
+   * \returns The target frequency
+   */
+  double getTargetFrequency() const
+  {
+    return target_frequency_;
+  }
+
   /*!
    * \brief Getter for the UR control version received from the robot.
    *
@@ -208,7 +218,7 @@ class RTDEClient
   void disconnect();
 
   /*!
-   * \brief Checks wheter the robot is booted, this is done by looking at the timestamp from the robot controller, this
+   * \brief Checks whether the robot is booted, this is done by looking at the timestamp from the robot controller, this
    * will show the time in seconds since the controller was started. If the timestamp is below 40, we will read from
    * the stream for approximately 1 second to ensure that the RTDE interface is up and running. This will ensure that we
    * don't finalize setting up communication, before the controller is up and running. Else we could end up connecting

include/ur_client_library/rtde/rtde_writer.h

@@ -61,7 +61,6 @@ class RTDEWriter
   ~RTDEWriter()
   {
     running_ = false;
-    std::this_thread::sleep_for(std::chrono::seconds(5));
     if (writer_thread_.joinable())
     {
       writer_thread_.join();
@@ -118,7 +117,7 @@ class RTDEWriter
    * \brief Creates a package to request setting a new value for one of the standard analog output pins.
    *
    * \param output_pin The pin to change
-   * \param value The new value
+   * \param value The new value, it should be between 0 and 1, where 0 is 4mA and 1 is 20mA.
    *
    * \returns Success of the package creation
    */

src/rtde/rtde_client.cpp

@@ -254,6 +254,11 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version)
   }
   else
   {
+    if (target_frequency_ != max_frequency_)
+    {
+      URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency "
+                    "equivalent to the maximum frequency will be used instead.");
+    }
     size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_);
   }
 
@@ -403,7 +408,9 @@ bool RTDEClient::isRobotBooted()
 
   while (timestamp < 40 && reading_count < target_frequency_ * 2)
   {
-    if (pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    // Set timeout based on target frequency, to make sure that reading doesn't timeout
+    int timeout = static_cast<int>((1 / target_frequency_) * 1000) * 10;
+    if (pipeline_.getLatestProduct(package, std::chrono::milliseconds(timeout)))
     {
       rtde_interface::DataPackage* tmp_input = dynamic_cast<rtde_interface::DataPackage*>(package.get());
       tmp_input->getData("timestamp", timestamp);
@@ -520,35 +527,24 @@ bool RTDEClient::sendPause()
     URCL_LOG_ERROR("Sending RTDE pause command failed!");
     return false;
   }
-  static unsigned num_retries = 0;
-  while (num_retries < MAX_REQUEST_RETRIES)
+  std::unique_ptr<RTDEPackage> package;
+  std::chrono::time_point start = std::chrono::steady_clock::now();
+  int seconds = 5;
+  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds))
   {
-    std::unique_ptr<RTDEPackage> package;
-
     if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot");
       return false;
     }
-
     if (rtde_interface::ControlPackagePause* tmp = dynamic_cast<rtde_interface::ControlPackagePause*>(package.get()))
     {
       client_state_ = ClientState::PAUSED;
       return tmp->accepted_;
     }
-    else
-    {
-      std::stringstream ss;
-      ss << "Did not receive answer to RTDE pause request. Message received instead: " << std::endl
-         << package->toString();
-      URCL_LOG_WARN("%s", ss.str().c_str());
-      num_retries++;
-    }
   }
   std::stringstream ss;
-  ss << "Could not pause RTDE communication after " << MAX_REQUEST_RETRIES
-     << " tries. Please check the output of the "
-        "negotiation attempts above to get a hint what could be wrong.";
+  ss << "Could not receive answer to pause RTDE communication after " << seconds << " seconds.";
   throw UrException(ss.str());
 }
 

src/rtde/rtde_writer.cpp

@@ -65,6 +65,15 @@ void RTDEWriter::run()
 
 bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
 {
+  if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0)
+  {
+    std::stringstream ss;
+    ss << "Speed slider fraction should be between 0 and 1. The speed slider fraction is "
+       << static_cast<int>(speed_slider_fraction);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint32_t mask = 1;
   bool success = true;
@@ -85,6 +94,14 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction)
 
 bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
 {
+  if (output_pin > 7)
+  {
+    std::stringstream ss;
+    ss << "Standard digital output pins goes from 0 to 7. The output pin to change is " << static_cast<int>(output_pin);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint8_t mask = pinToMask(output_pin);
   bool success = true;
@@ -114,6 +131,15 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value)
 
 bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
 {
+  if (output_pin > 7)
+  {
+    std::stringstream ss;
+    ss << "Configurable digital output pins goes from 0 to 7. The output pin to change is "
+       << static_cast<int>(output_pin);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint8_t mask = pinToMask(output_pin);
   bool success = true;
@@ -143,6 +169,14 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value)
 
 bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
 {
+  if (output_pin > 1)
+  {
+    std::stringstream ss;
+    ss << "Tool digital output pins goes from 0 to 1. The output pin to change is " << static_cast<int>(output_pin);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint8_t mask = pinToMask(output_pin);
   bool success = true;
@@ -172,6 +206,21 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value)
 
 bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value)
 {
+  if (output_pin > 1)
+  {
+    std::stringstream ss;
+    ss << "Standard analog output goes from 0 to 1. The output pin to change is " << static_cast<int>(output_pin);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+  if (value > 1.0 || value < 0.0)
+  {
+    std::stringstream ss;
+    ss << "Analog output value should be between 0 and 1. The value is " << static_cast<int>(value);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   uint8_t mask = pinToMask(output_pin);
   // default to current for now, as no functionality to choose included in set io service
@@ -206,6 +255,14 @@ uint8_t RTDEWriter::pinToMask(uint8_t pin)
 
 bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value)
 {
+  if (register_id < 64 || register_id > 127)
+  {
+    std::stringstream ss;
+    ss << "Input bit register goes from 64 to 127. The register id to change is " << static_cast<int>(register_id);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   std::stringstream ss;
   ss << "input_bit_register_" << register_id;
@@ -224,6 +281,14 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value)
 
 bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value)
 {
+  if (register_id < 24 || register_id > 47)
+  {
+    std::stringstream ss;
+    ss << "Input int register goes from 24 to 47. The register id to change is " << static_cast<int>(register_id);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   std::stringstream ss;
   ss << "input_int_register_" << register_id;
@@ -242,6 +307,14 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value)
 
 bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value)
 {
+  if (register_id < 24 || register_id > 47)
+  {
+    std::stringstream ss;
+    ss << "Input double register goes from 24 to 47. The register id to change is " << static_cast<int>(register_id);
+    URCL_LOG_ERROR(ss.str().c_str());
+    return false;
+  }
+
   std::lock_guard<std::mutex> guard(package_mutex_);
   std::stringstream ss;
   ss << "input_double_register_" << register_id;

tests/CMakeLists.txt

@@ -48,11 +48,11 @@ target_link_libraries(primary_parser_tests PRIVATE ur_client_library::urcl ${GTE
 gtest_add_tests(TARGET      primary_parser_tests
 )
 
-add_executable(rtde_data_package test_rtde_data_package.cpp)
-target_compile_options(rtde_data_package PRIVATE ${CXX17_FLAG})
-target_include_directories(rtde_data_package PRIVATE ${GTEST_INCLUDE_DIRS})
-target_link_libraries(rtde_data_package PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
-gtest_add_tests(TARGET      rtde_data_package
+add_executable(rtde_data_package_tests test_rtde_data_package.cpp)
+target_compile_options(rtde_data_package_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_data_package_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_data_package_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET      rtde_data_package_tests
 )
 
 add_executable(rtde_parser_tests test_rtde_parser.cpp)
@@ -89,3 +89,53 @@ target_include_directories(trajectory_point_interface_tests PRIVATE ${GTEST_INCL
 target_link_libraries(trajectory_point_interface_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
 gtest_add_tests(TARGET trajectory_point_interface_tests
 )
+
+add_executable(rtde_control_package_pause_tests test_rtde_control_package_pause.cpp)
+target_compile_options(rtde_control_package_pause_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_control_package_pause_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_control_package_pause_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_control_package_pause_tests
+)
+
+add_executable(rtde_control_package_start_tests test_rtde_control_package_start.cpp)
+target_compile_options(rtde_control_package_start_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_control_package_start_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_control_package_start_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_control_package_start_tests
+)
+
+add_executable(rtde_control_package_setup_outputs_tests test_rtde_control_package_setup_outputs.cpp)
+target_compile_options(rtde_control_package_setup_outputs_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_control_package_setup_outputs_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_control_package_setup_outputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_control_package_setup_outputs_tests
+)
+
+add_executable(rtde_control_package_setup_inputs_tests test_rtde_control_package_setup_inputs.cpp)
+target_compile_options(rtde_control_package_setup_inputs_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_control_package_setup_inputs_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_control_package_setup_inputs_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_control_package_setup_inputs_tests
+)
+
+add_executable(rtde_get_urcontrol_version_tests test_rtde_get_urcontrol_version.cpp)
+target_compile_options(rtde_get_urcontrol_version_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_get_urcontrol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_get_urcontrol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_get_urcontrol_version_tests
+)
+
+add_executable(rtde_request_protocol_version_tests test_rtde_request_protocol_version.cpp)
+target_compile_options(rtde_request_protocol_version_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_request_protocol_version_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_request_protocol_version_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_request_protocol_version_tests
+)
+
+add_executable(rtde_writer_tests test_rtde_writer.cpp)
+target_compile_options(rtde_writer_tests PRIVATE ${CXX17_FLAG})
+target_include_directories(rtde_writer_tests PRIVATE ${GTEST_INCLUDE_DIRS})
+target_link_libraries(rtde_writer_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES})
+gtest_add_tests(TARGET rtde_writer_tests
+)
+