From c98ad776f229695eb5a3a024b2532308b9532ce6 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Fri, 14 Nov 2025 16:01:59 +0100 Subject: [PATCH 01/16] Added support for std::vector strings in UrDriver --- include/ur_client_library/ur/ur_driver.h | 10 ++++++---- src/ur/ur_driver.cpp | 7 +++++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 15a4fd25..a78b196c 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -52,10 +52,12 @@ namespace urcl */ struct UrDriverConfiguration { - std::string robot_ip; //!< IP-address under which the robot is reachable. - std::string script_file; //!< URScript file that should be sent to the robot. - std::string output_recipe_file; //!< Filename where the output recipe is stored in. - std::string input_recipe_file; //!< Filename where the input recipe is stored in. + std::string robot_ip; //!< IP-address under which the robot is reachable. + std::string script_file; //!< URScript file that should be sent to the robot. + std::string output_recipe_file; //!< Filename where the output recipe is stored in. + std::string input_recipe_file; //!< Filename where the input recipe is stored in. + std::vector output_recipe; //!< Vector with the output recipe fields. + std::vector input_recipe; //!< Vector with the input recipe fields. /*! * \brief Function handle to a callback on program state changes. diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e860722e..9fc63601 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -76,8 +76,11 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset( - new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); + if (config.output_recipe_file.empty() && config.input_recipe_file.empty()) + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe, config.input_recipe)); + else + rtde_client_.reset( + new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); From 7d0c16afbe331dbb5556a4c7fde4989a36e77a3f Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Thu, 27 Nov 2025 10:09:14 +0100 Subject: [PATCH 02/16] Improved logic to handle recipe input/output file/vector --- include/ur_client_library/rtde/rtde_client.h | 6 +- include/ur_client_library/ur/ur_driver.h | 1 + src/rtde/rtde_client.cpp | 2 +- src/ur/ur_driver.cpp | 80 ++++++++++++++++---- 4 files changed, 69 insertions(+), 20 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index af152e4c..8d6c48ea 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -222,6 +222,9 @@ class RTDEClient return output_recipe_; } + // Reads output or input recipe from a file + static std::vector readRecipe(const std::string& recipe_file); + private: comm::URStream stream_; std::vector output_recipe_; @@ -247,9 +250,6 @@ class RTDEClient constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; - // Reads output or input recipe from a file - std::vector readRecipe(const std::string& recipe_file) const; - // Helper function to ensure that timestamp is present in the output recipe. The timestamp is needed to ensure that // the robot is booted. std::vector ensureTimestampIsPresent(const std::vector& output_recipe) const; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index a78b196c..7e7c3a47 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -989,6 +989,7 @@ class UrDriver private: void init(const UrDriverConfiguration& config); + void handleRTDEReset(const UrDriverConfiguration& config); void initRTDE(); void setupReverseInterface(const uint32_t reverse_port); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 6a230684..4ed2f3cd 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -664,7 +664,7 @@ bool RTDEClient::sendPause() throw UrException(ss.str()); } -std::vector RTDEClient::readRecipe(const std::string& recipe_file) const +std::vector RTDEClient::readRecipe(const std::string& recipe_file) { std::vector recipe; std::ifstream file(recipe_file); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9fc63601..dc60946b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -32,6 +32,7 @@ //---------------------------------------------------------------------- #include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/control/script_reader.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" @@ -39,6 +40,8 @@ #include "ur_client_library/helpers.h" #include #include +#include +#include #include @@ -76,11 +79,7 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - if (config.output_recipe_file.empty() && config.input_recipe_file.empty()) - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe, config.input_recipe)); - else - rtde_client_.reset( - new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); + handleRTDEReset(config); primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); @@ -129,7 +128,8 @@ void UrDriver::init(const UrDriverConfiguration& config) { if (robot_version_.major < 5) { - throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support using " + throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support " + "using " "the tool communication interface. Please check your configuration.", VersionInformation::fromString("5.0.0"), robot_version_); } @@ -171,7 +171,8 @@ void UrDriver::init(const UrDriverConfiguration& config) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " - "notice is for application developers using this library. If you are only using an application using " + "notice is for application developers using this library. If you are only using an application " + "using " "this library, you can ignore this message."); if (checkCalibration(config.calibration_checksum)) { @@ -179,11 +180,15 @@ void UrDriver::init(const UrDriverConfiguration& config) } else { - URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " - "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use " - "the ur_calibration tool to extract the correct calibration from the robot and pass that into the " + URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given " + "kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. " + "Use " + "the ur_calibration tool to extract the correct calibration from the robot and pass that into " + "the " "description. See " - "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "[https://github.com/UniversalRobots/" + "Universal_Robots_ROS_Driver#extract-calibration-information] " "for details."); } } @@ -192,8 +197,8 @@ void UrDriver::init(const UrDriverConfiguration& config) std::unique_ptr urcl::UrDriver::getDataPackage() { - // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control - // loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with + // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the + // control loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with // something else (combined_robot_hw) std::chrono::milliseconds timeout(get_packet_timeout_); @@ -258,7 +263,8 @@ bool UrDriver::zeroFTSensor() if (getVersion().major < 5) { std::stringstream ss; - ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This robot's " + ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This " + "robot's " "version is " << getVersion(); URCL_LOG_ERROR(ss.str().c_str()); @@ -629,9 +635,11 @@ std::vector UrDriver::getRTDEOutputRecipe() void UrDriver::setKeepaliveCount(const uint32_t count) { URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the " - "RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to " + "RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code " + "to " "set the " - "read timeout in the write commands directly. This keepalive count will overwrite the timeout passed " + "read timeout in the write commands directly. This keepalive count will overwrite the timeout " + "passed " "to the write functions."); // TODO: Remove 2027-05 #pragma GCC diagnostic push @@ -687,4 +695,44 @@ std::deque UrDriver::getErrorCodes() { return primary_client_->getErrorCodes(); } + +void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) +{ + bool use_output_file = true; + if (config.output_recipe_file.empty() && config.output_recipe.size() == 0) + throw UrException("Neither output recipe file nor output recipe have been defined"); + else if (!config.output_recipe_file.empty() && config.output_recipe.size() != 0) + URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe file"); + else if (config.output_recipe_file.empty()) + use_output_file = false; + if (use_output_file && !std::filesystem::exists(config.output_recipe_file)) + throw UrException("Output recipe file does not exist: " + config.output_recipe_file); + + bool use_input_file = true; + if (config.input_recipe_file.empty() && config.input_recipe.size() == 0) + throw UrException("Neither input recipe file nor input recipe have been defined"); + else if (!config.input_recipe_file.empty() && config.input_recipe.size() != 0) + URCL_LOG_WARN("Both input recipe file and input recipe vector are used. Defaulting to input recipe file"); + else if (config.input_recipe_file.empty()) + use_input_file = false; + + if (use_input_file && !std::filesystem::exists(config.input_recipe_file)) + throw UrException("Input recipe file does not exist: " + config.input_recipe_file); + + if (use_input_file && use_output_file) + rtde_client_.reset( + new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); + else + { + auto input_recipe = config.input_recipe; + auto output_recipe = config.output_recipe; + if (use_input_file) + input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); + if (use_output_file) + output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); + + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe)); + } +} + } // namespace urcl From e11e60b1bdaf87b6d8646a3395049ce1328ead24 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Thu, 27 Nov 2025 15:20:06 +0100 Subject: [PATCH 03/16] Added additional constructors to ExampleRobotWrapper --- .../ur_client_library/example_robot_wrapper.h | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 36831a12..38b422e9 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -76,6 +76,26 @@ class ExampleRobotWrapper ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file, const std::string& input_recipe_file, const bool headless_mode = true, const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE); + /*! + * \brief Construct a new Example Robot Wrapper object + * + * This will connect to a robot and initialize it. In headless mode the program will be running + * instantly, in teach pendant mode the from \p autostart_program will be started. + * + * Note: RTDE communication has to be started separately. + * + * \param robot_ip IP address of the robot to connect to + * \param output_recipe Output recipe vector for RTDE communication + * \param input_recipe Input recipe vector for RTDE communication + * \param headless_mode Should the driver be started in headless mode or not? + * \param autostart_program Program to start automatically after initialization when not in + * headless mode. This flag is ignored in headless mode. + * \param script_file URScript file to send to the robot. That should be script code + * communicating to the driver's reverse interface and trajectory interface. + */ + ExampleRobotWrapper(const std::string& robot_ip, const std::vector output_recipe, + const std::vector input_recipe, const bool headless_mode = true, + const std::string& autostart_program = "", const std::string& script_file = SCRIPT_FILE); ~ExampleRobotWrapper(); /** From ad96a0afafc613c7a5f56b099adc655e7502c411 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Mon, 1 Dec 2025 11:03:42 +0100 Subject: [PATCH 04/16] Added getter input recipe for Driver. Added init tests for UrDriver --- include/ur_client_library/rtde/rtde_client.h | 10 +++ include/ur_client_library/ur/ur_driver.h | 7 ++ src/ur/ur_driver.cpp | 5 ++ tests/test_ur_driver.cpp | 85 ++++++++++++++++++++ 4 files changed, 107 insertions(+) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 8d6c48ea..5ebc76ce 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -222,6 +222,16 @@ class RTDEClient return output_recipe_; } + /*! + * \brief Getter for the RTDE input recipe. + * + * \returns The input recipe + */ + std::vector getInputRecipe() + { + return input_recipe_; + } + // Reads output or input recipe from a file static std::vector readRecipe(const std::string& recipe_file); diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 7e7c3a47..4c0ed13d 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -839,6 +839,13 @@ class UrDriver */ std::vector getRTDEOutputRecipe(); + /*! + * \brief Getter for the RTDE input recipe used in the RTDE client. + * + * \returns The used RTDE input recipe + */ + std::vector getRTDEInputRecipe(); + /*! * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot. * diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index dc60946b..52f3c2d1 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -632,6 +632,11 @@ std::vector UrDriver::getRTDEOutputRecipe() return rtde_client_->getOutputRecipe(); } +std::vector UrDriver::getRTDEInputRecipe() +{ + return rtde_client_->getInputRecipe(); +} + void UrDriver::setKeepaliveCount(const uint32_t count) { URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the " diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 80713157..09865754 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include "test_utils.h" using namespace urcl; @@ -40,6 +41,43 @@ using namespace urcl; const std::string SCRIPT_FILE = "../resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +const std::vector OUTPUT_RECIPE_VECTOR = { "timestamp", + "actual_qd", + "speed_scaling", + "target_speed_fraction", + "runtime_state", + "actual_TCP_force", + "actual_TCP_pose", + "actual_digital_input_bits", + "actual_digital_output_bits", + "standard_analog_input0", + "standard_analog_input1", + "standard_analog_output0", + "standard_analog_output1", + "analog_io_types", + "tool_mode", + "tool_analog_input_types", + "tool_analog_input0", + "tool_analog_input1", + "tool_output_voltage", + "tool_output_current", + "tool_temperature", + "robot_mode", + "safety_mode", + "robot_status_bits", + "safety_status_bits", + "actual_current", + "tcp_offset" }; +const std::vector INPUT_RECIPE_VECTOR = { + "speed_slider_mask", "standard_digital_output_mask", + "standard_digital_output", "configurable_digital_output_mask", + "configurable_digital_output", "tool_digital_output_mask", + "tool_digital_output", "standard_analog_output_mask", + "standard_analog_output_type", "standard_analog_output_0", + "standard_analog_output_1" +}; +const std::string OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "actual_q"; +const std::string INPUT_RECIPE_VECTOR_EXCLUDED_VALUE = "speed_slider_fraction"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; @@ -310,6 +348,53 @@ TEST(UrDriverInitTest, setting_connection_limits_works_correctly) EXPECT_THROW(UrDriver ur_driver(config), UrException); } +TEST(UrDriverInitTest, no_recipe_throws_error) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); +} + +TEST(UrDriverInitTest, non_existing_recipe_file_throws_exception) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.input_recipe_file = " "; + config.output_recipe_file = OUTPUT_RECIPE; + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); + + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = " "; +} + +TEST(UrDriverInitTest, both_recipe_file_and_vector_select_file) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = OUTPUT_RECIPE; + config.input_recipe = INPUT_RECIPE_VECTOR; + config.output_recipe = OUTPUT_RECIPE_VECTOR; + config.headless_mode = g_HEADLESS; + + auto driver = UrDriver(config); + auto output_recipe = driver.getRTDEOutputRecipe(); + EXPECT_TRUE(std::find(output_recipe.begin(), output_recipe.end(), OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE) != + output_recipe.end()); + auto input_recipe = driver.getRTDEInputRecipe(); + EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) != + output_recipe.end()); +} // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[]) From 7c6d95a2b2227bb1312fa1cd2c2378fcb157df69 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Mon, 1 Dec 2025 11:21:20 +0100 Subject: [PATCH 05/16] Update src/ur/ur_driver.cpp Co-authored-by: Felix Exner --- src/ur/ur_driver.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 52f3c2d1..e0988070 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -703,15 +703,20 @@ std::deque UrDriver::getErrorCodes() void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) { - bool use_output_file = true; + auto output_recipe = config.output_recipe; if (config.output_recipe_file.empty() && config.output_recipe.size() == 0) - throw UrException("Neither output recipe file nor output recipe have been defined"); - else if (!config.output_recipe_file.empty() && config.output_recipe.size() != 0) - URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe file"); - else if (config.output_recipe_file.empty()) - use_output_file = false; - if (use_output_file && !std::filesystem::exists(config.output_recipe_file)) - throw UrException("Output recipe file does not exist: " + config.output_recipe_file); + { + throw UrException("Neither output recipe file nor output recipe have been defined. An output recipe is required."); + } + if (!config.output_recipe_file.empty()) + { + if (config.output_recipe.size() != 0) + URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe vector"); + } + else + { + output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); + } bool use_input_file = true; if (config.input_recipe_file.empty() && config.input_recipe.size() == 0) From 43f5dab46827deadfbf4c10c2ca3a1b091071be0 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Mon, 1 Dec 2025 11:21:33 +0100 Subject: [PATCH 06/16] Update src/ur/ur_driver.cpp Co-authored-by: Felix Exner --- src/ur/ur_driver.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e0988070..9afd5e0d 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -718,13 +718,17 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); } - bool use_input_file = true; - if (config.input_recipe_file.empty() && config.input_recipe.size() == 0) - throw UrException("Neither input recipe file nor input recipe have been defined"); - else if (!config.input_recipe_file.empty() && config.input_recipe.size() != 0) - URCL_LOG_WARN("Both input recipe file and input recipe vector are used. Defaulting to input recipe file"); - else if (config.input_recipe_file.empty()) - use_input_file = false; + auto input_recipe = config.input_recipe; + if (!config.input_recipe_file.empty()) + { + if (config.input_recipe.size() != 0) + { + URCL_LOG_WARN("Both input recipe file and input recipe vector are used. Defaulting to input recipe vector."); + } + else + { + input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); + } if (use_input_file && !std::filesystem::exists(config.input_recipe_file)) throw UrException("Input recipe file does not exist: " + config.input_recipe_file); From 4c1afeefa81bcf0663137f5c6cb413da42cf99be Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Mon, 1 Dec 2025 11:21:41 +0100 Subject: [PATCH 07/16] Update src/ur/ur_driver.cpp Co-authored-by: Felix Exner --- src/ur/ur_driver.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9afd5e0d..1d9c51d0 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -730,21 +730,6 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); } - if (use_input_file && !std::filesystem::exists(config.input_recipe_file)) - throw UrException("Input recipe file does not exist: " + config.input_recipe_file); - - if (use_input_file && use_output_file) - rtde_client_.reset( - new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); - else - { - auto input_recipe = config.input_recipe; - auto output_recipe = config.output_recipe; - if (use_input_file) - input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); - if (use_output_file) - output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe)); } } From 37f300f9e827f536b18bdb28c4c9fe06522784c1 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Mon, 1 Dec 2025 11:28:52 +0100 Subject: [PATCH 08/16] Fix tests --- tests/test_ur_driver.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 09865754..e09b12ad 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -359,23 +359,32 @@ TEST(UrDriverInitTest, no_recipe_throws_error) EXPECT_THROW(UrDriver ur_driver(config), UrException); } -TEST(UrDriverInitTest, non_existing_recipe_file_throws_exception) +TEST(UrDriverInitTest, non_existing_output_recipe_file_throws_exception) { UrDriverConfiguration config; config.socket_reconnect_attempts = 1; config.socket_reconnection_timeout = std::chrono::milliseconds(200); config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network - config.input_recipe_file = " "; - config.output_recipe_file = OUTPUT_RECIPE; + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = " "; config.headless_mode = g_HEADLESS; EXPECT_THROW(UrDriver ur_driver(config), UrException); +} - config.input_recipe_file = INPUT_RECIPE; - config.output_recipe_file = " "; +TEST(UrDriverInitTest, non_existing_input_recipe_file_does_not_throw_exception) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network + config.output_recipe_file = OUTPUT_RECIPE; + config.headless_mode = g_HEADLESS; + + EXPECT_NO_THROW(THROW(UrDriver ur_driver(config)); } -TEST(UrDriverInitTest, both_recipe_file_and_vector_select_file) +TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector) { UrDriverConfiguration config; config.socket_reconnect_attempts = 1; @@ -389,10 +398,10 @@ TEST(UrDriverInitTest, both_recipe_file_and_vector_select_file) auto driver = UrDriver(config); auto output_recipe = driver.getRTDEOutputRecipe(); - EXPECT_TRUE(std::find(output_recipe.begin(), output_recipe.end(), OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE) != + EXPECT_TRUE(std::find(output_recipe.begin(), output_recipe.end(), OUTPUT_RECIPE_VECTOR_EXCLUDED_VALUE) == output_recipe.end()); auto input_recipe = driver.getRTDEInputRecipe(); - EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) != + EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) == output_recipe.end()); } // TODO we should add more tests for the UrDriver class. @@ -417,4 +426,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} From a715c62fb79d3e5acf019bc9044dc31868b246b9 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Tue, 2 Dec 2025 08:57:40 +0100 Subject: [PATCH 09/16] Fix clang-format --- src/ur/ur_driver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 1d9c51d0..dd9267dc 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -710,8 +710,8 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) } if (!config.output_recipe_file.empty()) { - if (config.output_recipe.size() != 0) - URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe vector"); + if (config.output_recipe.size() != 0) + URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe vector"); } else { From dee0eedec70d04ab3006e6155cd3e8eec4967d99 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 14:58:48 +0100 Subject: [PATCH 10/16] Fix logic --- src/ur/ur_driver.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index dd9267dc..3a272312 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -711,11 +711,13 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) if (!config.output_recipe_file.empty()) { if (config.output_recipe.size() != 0) + { URCL_LOG_WARN("Both output recipe file and output recipe vector are used. Defaulting to output recipe vector"); - } - else - { - output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); + } + else + { + output_recipe = rtde_interface::RTDEClient::readRecipe(config.output_recipe_file); + } } auto input_recipe = config.input_recipe; @@ -729,9 +731,9 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) { input_recipe = rtde_interface::RTDEClient::readRecipe(config.input_recipe_file); } - - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe)); } + + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe)); } -} // namespace urcl +} // namespace urcl \ No newline at end of file From 8fbef2fe413062bbfe7653c1acbd43e49d37ae6f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 14:58:57 +0100 Subject: [PATCH 11/16] Fix test syntax --- tests/test_ur_driver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index e09b12ad..7575a4f1 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -381,7 +381,7 @@ TEST(UrDriverInitTest, non_existing_input_recipe_file_does_not_throw_exception) config.output_recipe_file = OUTPUT_RECIPE; config.headless_mode = g_HEADLESS; - EXPECT_NO_THROW(THROW(UrDriver ur_driver(config)); + EXPECT_NO_THROW(UrDriver ur_driver(config)); } TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector) @@ -426,4 +426,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} +} \ No newline at end of file From 381ccf726c4fe23ff5d557378ed9682cdf7f5daa Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 15:23:20 +0100 Subject: [PATCH 12/16] Allow not setting a handle_program_state callback --- include/ur_client_library/ur/ur_driver.h | 4 ++-- src/control/reverse_interface.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 4c0ed13d..7c9a0315 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -65,7 +65,7 @@ struct UrDriverConfiguration * For this to work, the URScript program will have to send keepalive signals to the \p * reverse_port. */ - std::function handle_program_state; + std::function handle_program_state = nullptr; bool headless_mode; //!< Parameter to control if the driver should be started in headless mode. std::unique_ptr tool_comm_setup = nullptr; //!< Configuration for using the tool communication. @@ -1033,4 +1033,4 @@ class UrDriver VersionInformation robot_version_; }; } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED \ No newline at end of file diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index d9fec1c7..5ffd186f 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -47,7 +47,10 @@ ReverseInterface::ReverseInterface(const ReverseInterfaceConfig& config) , step_time_(config.step_time) , keep_alive_count_modified_deprecated_(false) { - handle_program_state_(false); + if (handle_program_state_) + { + handle_program_state_(false); + } server_.setMessageCallback(std::bind(&ReverseInterface::messageCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); server_.setConnectCallback(std::bind(&ReverseInterface::connectionCallback, this, std::placeholders::_1)); @@ -251,4 +254,4 @@ void ReverseInterface::messageCallback(const socket_t filedescriptor, char* buff } } // namespace control -} // namespace urcl +} // namespace urcl \ No newline at end of file From ca871c4678866776899cc0d13fbc7e096a01635a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 15:23:31 +0100 Subject: [PATCH 13/16] Fix tests --- tests/test_ur_driver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 7575a4f1..ccf2af2f 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -380,6 +380,7 @@ TEST(UrDriverInitTest, non_existing_input_recipe_file_does_not_throw_exception) config.robot_ip = g_ROBOT_IP; // That IP address should not exist on the test network config.output_recipe_file = OUTPUT_RECIPE; config.headless_mode = g_HEADLESS; + config.script_file = SCRIPT_FILE; EXPECT_NO_THROW(UrDriver ur_driver(config)); } @@ -395,6 +396,7 @@ TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector) config.input_recipe = INPUT_RECIPE_VECTOR; config.output_recipe = OUTPUT_RECIPE_VECTOR; config.headless_mode = g_HEADLESS; + config.script_file = SCRIPT_FILE; auto driver = UrDriver(config); auto output_recipe = driver.getRTDEOutputRecipe(); @@ -402,7 +404,7 @@ TEST(UrDriverInitTest, both_recipe_file_and_vector_select_vector) output_recipe.end()); auto input_recipe = driver.getRTDEInputRecipe(); EXPECT_TRUE(std::find(input_recipe.begin(), input_recipe.end(), INPUT_RECIPE_VECTOR_EXCLUDED_VALUE) == - output_recipe.end()); + input_recipe.end()); } // TODO we should add more tests for the UrDriver class. From 18e889c5e3419625ce740db2704eae5625433a00 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 15:48:43 +0100 Subject: [PATCH 14/16] Extend handle_program_state optional --- src/control/reverse_interface.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index 5ffd186f..356cd09a 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -227,7 +227,10 @@ void ReverseInterface::connectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Robot connected to reverse interface. Ready to receive control commands."); client_fd_ = filedescriptor; - handle_program_state_(true); + if (handle_program_state_) + { + handle_program_state_(true); + } } else { @@ -240,7 +243,10 @@ void ReverseInterface::disconnectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Connection to reverse interface dropped.", filedescriptor); client_fd_ = INVALID_SOCKET; - handle_program_state_(false); + if (handle_program_state_) + { + handle_program_state_(false); + } for (auto handler : disconnect_callbacks_) { handler.function(filedescriptor); From 9c0f108603515f7d735446e8fb4c633a32c13839 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 2 Dec 2025 15:57:30 +0100 Subject: [PATCH 15/16] Rename handleRTDEReset to setupRTDEClient and call initRTDE from the inside --- include/ur_client_library/ur/ur_driver.h | 2 +- src/ur/ur_driver.cpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 7c9a0315..5aaf5314 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -996,7 +996,7 @@ class UrDriver private: void init(const UrDriverConfiguration& config); - void handleRTDEReset(const UrDriverConfiguration& config); + void setupRTDEClient(const UrDriverConfiguration& config); void initRTDE(); void setupReverseInterface(const uint32_t reverse_port); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 3a272312..81d65b57 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -79,13 +79,12 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - handleRTDEReset(config); primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); get_packet_timeout_ = non_blocking_read_ ? 0 : 100; - initRTDE(); + setupRTDEClient(config); setupReverseInterface(config.reverse_port); // Figure out the ip automatically if the user didn't provide it @@ -701,7 +700,7 @@ std::deque UrDriver::getErrorCodes() return primary_client_->getErrorCodes(); } -void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) +void UrDriver::setupRTDEClient(const UrDriverConfiguration& config) { auto output_recipe = config.output_recipe; if (config.output_recipe_file.empty() && config.output_recipe.size() == 0) @@ -733,7 +732,7 @@ void UrDriver::handleRTDEReset(const UrDriverConfiguration& config) } } - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe)); + resetRTDEClient(output_recipe, input_recipe); } } // namespace urcl \ No newline at end of file From 15e3c720daa2718fe761f433e908d99ff00cda3f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 3 Dec 2025 08:13:40 +0100 Subject: [PATCH 16/16] Revert unrelated string formatting changes --- src/ur/ur_driver.cpp | 33 ++++++++++++--------------------- 1 file changed, 12 insertions(+), 21 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 81d65b57..e1dd6e0b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -127,8 +127,7 @@ void UrDriver::init(const UrDriverConfiguration& config) { if (robot_version_.major < 5) { - throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support " - "using " + throw ToolCommNotAvailable("Tool communication setup requested, but this robot version does not support using " "the tool communication interface. Please check your configuration.", VersionInformation::fromString("5.0.0"), robot_version_); } @@ -170,8 +169,7 @@ void UrDriver::init(const UrDriverConfiguration& config) { URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " - "notice is for application developers using this library. If you are only using an application " - "using " + "notice is for application developers using this library. If you are only using an application using " "this library, you can ignore this message."); if (checkCalibration(config.calibration_checksum)) { @@ -179,15 +177,11 @@ void UrDriver::init(const UrDriverConfiguration& config) } else { - URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given " - "kinematics " - "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. " - "Use " - "the ur_calibration tool to extract the correct calibration from the robot and pass that into " - "the " + URCL_LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use " + "the ur_calibration tool to extract the correct calibration from the robot and pass that into the " "description. See " - "[https://github.com/UniversalRobots/" - "Universal_Robots_ROS_Driver#extract-calibration-information] " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " "for details."); } } @@ -196,8 +190,8 @@ void UrDriver::init(const UrDriverConfiguration& config) std::unique_ptr urcl::UrDriver::getDataPackage() { - // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the - // control loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with + // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control + // loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with // something else (combined_robot_hw) std::chrono::milliseconds timeout(get_packet_timeout_); @@ -262,8 +256,7 @@ bool UrDriver::zeroFTSensor() if (getVersion().major < 5) { std::stringstream ss; - ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This " - "robot's " + ss << "Zeroing the Force-Torque sensor is only available for e-Series robots (Major version >= 5). This robot's " "version is " << getVersion(); URCL_LOG_ERROR(ss.str().c_str()); @@ -639,11 +632,9 @@ std::vector UrDriver::getRTDEInputRecipe() void UrDriver::setKeepaliveCount(const uint32_t count) { URCL_LOG_WARN("DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the " - "RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code " - "to " + "RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to " "set the " - "read timeout in the write commands directly. This keepalive count will overwrite the timeout " - "passed " + "read timeout in the write commands directly. This keepalive count will overwrite the timeout passed " "to the write functions."); // TODO: Remove 2027-05 #pragma GCC diagnostic push @@ -735,4 +726,4 @@ void UrDriver::setupRTDEClient(const UrDriverConfiguration& config) resetRTDEClient(output_recipe, input_recipe); } -} // namespace urcl \ No newline at end of file +} // namespace urcl