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 1/4] 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 2/4] 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 58a35fea8a14f79e78dbe5a28ec990e140aea01d Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Thu, 27 Nov 2025 11:12:51 +0100 Subject: [PATCH 3/4] New tests for UrDriver recipe reading changes --- examples/direct_torque_control.cpp | 6 +- examples/external_fts_through_rtde.cpp | 4 +- examples/force_mode_example.cpp | 5 +- examples/freedrive_example.cpp | 5 +- examples/full_driver.cpp | 5 +- examples/instruction_executor.cpp | 7 +- examples/script_command_interface.cpp | 5 +- examples/spline_example.cpp | 5 +- examples/tool_contact_example.cpp | 5 +- examples/trajectory_point_interface.cpp | 5 +- .../ur_client_library/example_robot_wrapper.h | 3 +- src/example_robot_wrapper.cpp | 7 +- tests/test_instruction_executor.cpp | 12 ++-- tests/test_spline_interpolation.cpp | 42 ++++-------- tests/test_ur_driver.cpp | 68 ++++++++++++++++++- 15 files changed, 124 insertions(+), 60 deletions(-) diff --git a/examples/direct_torque_control.cpp b/examples/direct_torque_control.cpp index f8c789e9..da7a139e 100644 --- a/examples/direct_torque_control.cpp +++ b/examples/direct_torque_control.cpp @@ -30,6 +30,7 @@ #include #include +#include // In a real-world example it would be better to get those values from command line parameters / a // better configuration system such as Boost.Program_options @@ -37,6 +38,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; const size_t JOINT_INDEX = 5; // Joint index to control, in this case joint 6 (index 5) @@ -62,8 +64,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index f8838f09..f9a0f222 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -97,6 +97,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_only_ft.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; std::atomic g_RUNNING = true; @@ -271,7 +272,8 @@ int main(int argc, char* argv[]) second_to_run = std::stoi(argv[2]); } - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, true); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, true); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/examples/force_mode_example.cpp b/examples/force_mode_example.cpp index eb830d59..613a30d8 100644 --- a/examples/force_mode_example.cpp +++ b/examples/force_mode_example.cpp @@ -51,6 +51,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_robot; @@ -83,8 +84,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index e03e46af..622dedba 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -50,6 +50,7 @@ using namespace urcl; const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; @@ -82,8 +83,8 @@ int main(int argc, char* argv[]) bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index d123b523..abb69d2d 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -44,6 +44,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; vector6d_t g_joint_positions; @@ -60,8 +61,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/examples/instruction_executor.cpp b/examples/instruction_executor.cpp index 85fa5ade..05066530 100644 --- a/examples/instruction_executor.cpp +++ b/examples/instruction_executor.cpp @@ -44,6 +44,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::unique_ptr g_my_robot; @@ -59,8 +60,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, headless_mode, "external_control.urp"); if (!g_my_robot->getUrDriver()->checkCalibration(CALIBRATION_CHECKSUM)) { URCL_LOG_ERROR("Calibration checksum does not match actual robot."); @@ -113,4 +114,4 @@ int main(int argc, char* argv[]) g_my_robot->getUrDriver()->stopControl(); return 0; -} \ No newline at end of file +} diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2..d878520b 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -41,6 +41,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; bool g_HEADLESS = true; @@ -103,8 +104,8 @@ int main(int argc, char* argv[]) std::string(argv[3]) == "TRUE"; } - g_my_robot = - std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + g_HEADLESS, "external_control.urp"); if (!g_my_robot->isHealthy()) { diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index a29b2677..7266bbb1 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -47,6 +47,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; vector6d_t g_joint_positions; @@ -123,8 +124,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/examples/tool_contact_example.cpp b/examples/tool_contact_example.cpp index d16d399e..e5af9690 100644 --- a/examples/tool_contact_example.cpp +++ b/examples/tool_contact_example.cpp @@ -47,6 +47,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; std::atomic g_tool_contact_result_triggered; @@ -78,8 +79,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/examples/trajectory_point_interface.cpp b/examples/trajectory_point_interface.cpp index 7e2efe18..be3b4ad5 100644 --- a/examples/trajectory_point_interface.cpp +++ b/examples/trajectory_point_interface.cpp @@ -43,6 +43,7 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; std::unique_ptr g_my_robot; std::atomic g_trajectory_done = false; @@ -64,8 +65,8 @@ int main(int argc, char* argv[]) } bool headless_mode = true; - g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, - "external_control.urp"); + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, headless_mode, "external_control.urp"); if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 36831a12..462c1371 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -74,7 +74,8 @@ class ExampleRobotWrapper * communicating to the driver's reverse interface and trajectory interface. */ 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& input_recipe_file, 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(); diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index 34353d20..9d3e3b4f 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -30,6 +30,7 @@ #include #include +#include #include "ur_client_library/exceptions.h" #include "ur_client_library/log.h" #include "ur_client_library/ur/version_information.h" @@ -37,7 +38,9 @@ namespace urcl { ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std::string& output_recipe_file, - const std::string& input_recipe_file, const bool headless_mode, + const std::string& input_recipe_file, + const std::vector output_recipe, + const std::vector input_recipe, const bool headless_mode, const std::string& autostart_program, const std::string& script_file) : headless_mode_(headless_mode), autostart_program_(autostart_program) { @@ -73,6 +76,8 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: driver_config.script_file = script_file; driver_config.output_recipe_file = output_recipe_file; driver_config.input_recipe_file = input_recipe_file; + driver_config.output_recipe = output_recipe; + driver_config.input_recipe = input_recipe; driver_config.handle_program_state = std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1); driver_config.headless_mode = headless_mode; diff --git a/tests/test_instruction_executor.cpp b/tests/test_instruction_executor.cpp index f83dfd62..186bdbd1 100644 --- a/tests/test_instruction_executor.cpp +++ b/tests/test_instruction_executor.cpp @@ -45,6 +45,7 @@ using namespace urcl::control; 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 NULL_VECTOR = {}; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; @@ -63,8 +64,8 @@ class InstructionExecutorTest : public ::testing::Test GTEST_SKIP_("Running URCap tests for PolyScope X is currently not supported."); } // Setup driver - g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, - "external_control.urp", SCRIPT_FILE); + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, g_HEADLESS, "external_control.urp", SCRIPT_FILE); } void SetUp() override { @@ -273,8 +274,9 @@ TEST(InstructionExecutorTestStandalone, canceling_without_receiving_answer_retur } } out_file.close(); - auto my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, - "external_control.urp", test_script_file); + auto my_robot = + std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + g_HEADLESS, "external_control.urp", test_script_file); auto executor = std::make_unique(my_robot->getUrDriver()); my_robot->clearProtectiveStop(); // Make sure script is running on the robot @@ -475,4 +477,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 6c6630e6..277e08a1 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -50,6 +50,7 @@ const std::string SCRIPT_FILE = "../resources/external_control.urscript"; const std::string SPLINE_SCRIPT_FILE = "spline_external_control.urscript"; const std::string OUTPUT_RECIPE = "resources/rtde_output_recipe_spline.txt"; const std::string INPUT_RECIPE = "resources/rtde_input_recipe.txt"; +const std::vector NULL_VECTOR = {}; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; @@ -114,8 +115,9 @@ class SplineInterpolationTest : public ::testing::Test } // Setup driver - g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, - "external_control.urp", SPLINE_SCRIPT_FILE); + g_my_robot = + std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + g_HEADLESS, "external_control.urp", SPLINE_SCRIPT_FILE); g_my_robot->startRTDECommununication(true); @@ -292,34 +294,14 @@ class SplineInterpolationTest : public ::testing::Test { std::ofstream outfile(filename); // Header - outfile << "time, " - << "actual_positions0, " - << "actual_positions1, " - << "actual_positions2, " - << "actual_positions3, " - << "actual_positions4, " - << "actual_positions5, " - << "actual_velocities0, " - << "actual_velocities1, " - << "actual_velocities2, " - << "actual_velocities3, " - << "actual_velocities4, " - << "actual_velocities5, " - << "actual_acceleration0, " - << "actual_acceleration1, " - << "actual_acceleration2, " - << "actual_acceleration3, " - << "actual_acceleration4, " - << "actual_acceleration5, " - << "error_positions0, " - << "error_positions1, " - << "error_positions2, " - << "error_positions3, " - << "error_positions4, " - << "error_positions5, " - << "speed_scaling, " - << "spline_time" - << "\n"; + outfile << "time, " << "actual_positions0, " << "actual_positions1, " << "actual_positions2, " + << "actual_positions3, " << "actual_positions4, " << "actual_positions5, " << "actual_velocities0, " + << "actual_velocities1, " << "actual_velocities2, " << "actual_velocities3, " << "actual_velocities4, " + << "actual_velocities5, " << "actual_acceleration0, " << "actual_acceleration1, " + << "actual_acceleration2, " << "actual_acceleration3, " << "actual_acceleration4, " + << "actual_acceleration5, " << "error_positions0, " << "error_positions1, " << "error_positions2, " + << "error_positions3, " << "error_positions4, " << "error_positions5, " << "speed_scaling, " + << "spline_time" << "\n"; // Data for (unsigned int i = 0; i < actual_positions.size(); ++i) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 80713157..a7436ba6 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -40,6 +40,9 @@ 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" }; +const std::vector INPUT_RECIPE_VECTOR = { "speed_slider_mask" }; +const std::vector NULL_VECTOR = {}; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; std::string g_ROBOT_IP = "192.168.56.101"; bool g_HEADLESS = true; @@ -56,8 +59,8 @@ class UrDriverTest : public ::testing::Test GTEST_SKIP_("Running URCap tests for PolyScope X is currently not supported."); } // Setup driver - g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, - "external_control.urp", SCRIPT_FILE); + g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, g_HEADLESS, "external_control.urp", SCRIPT_FILE); g_my_robot->startRTDECommununication(true); } @@ -78,6 +81,65 @@ class UrDriverTest : public ::testing::Test } }; +TEST_F(UrDriverTest, read_bad_output_recipe) +{ + const std::string non_existing_file = ""; + EXPECT_THROW(g_my_robot = + std::make_unique(g_ROBOT_IP, non_existing_file, INPUT_RECIPE, NULL_VECTOR, + NULL_VECTOR, g_HEADLESS, "external_control.urp", SCRIPT_FILE), + UrException); +} + +TEST_F(UrDriverTest, read_bad_input_recipe) +{ + const std::string non_existing_file = ""; + EXPECT_THROW(g_my_robot = + std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, non_existing_file, NULL_VECTOR, + NULL_VECTOR, g_HEADLESS, "external_control.urp", SCRIPT_FILE), + UrException); +} + +TEST_F(UrDriverTest, read_existing_recipe_files) +{ + EXPECT_NO_THROW(g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, + NULL_VECTOR, NULL_VECTOR, g_HEADLESS, + "external_control.urp", SCRIPT_FILE)); +} + +TEST_F(UrDriverTest, read_existing_recipe_vectors) +{ + const std::string non_existing_file = ""; + EXPECT_NO_THROW(g_my_robot = std::make_unique(g_ROBOT_IP, non_existing_file, non_existing_file, + OUTPUT_RECIPE_VECTOR, INPUT_RECIPE_VECTOR, + g_HEADLESS, "external_control.urp", SCRIPT_FILE)); +} + +TEST_F(UrDriverTest, read_existing_recipe_files_and_vectors) +{ + const std::string non_existing_file = ""; + EXPECT_NO_THROW(g_my_robot = std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, INPUT_RECIPE, + OUTPUT_RECIPE_VECTOR, INPUT_RECIPE_VECTOR, + g_HEADLESS, "external_control.urp", SCRIPT_FILE)); +} + +TEST_F(UrDriverTest, read_false_existing_output_file) +{ + const std::string false_file = "false_file"; + EXPECT_THROW(g_my_robot = + std::make_unique(g_ROBOT_IP, false_file, INPUT_RECIPE, NULL_VECTOR, NULL_VECTOR, + g_HEADLESS, "external_control.urp", SCRIPT_FILE), + UrException); +} + +TEST_F(UrDriverTest, read_false_existing_recipe_file) +{ + const std::string false_file = "false_file"; + EXPECT_THROW(g_my_robot = + std::make_unique(g_ROBOT_IP, OUTPUT_RECIPE, false_file, NULL_VECTOR, + NULL_VECTOR, g_HEADLESS, "external_control.urp", SCRIPT_FILE), + UrException); +} + TEST_F(UrDriverTest, read_non_existing_script_file) { const std::string non_existing_script_file = ""; @@ -332,4 +394,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 4ae92191cc30b0d0fbcd989f73d73374d956fb61 Mon Sep 17 00:00:00 2001 From: Pablo David Aranda Rodriguez Date: Thu, 27 Nov 2025 15:20:06 +0100 Subject: [PATCH 4/4] Added additional constructors to ExampleRobotWrapper --- .../ur_client_library/example_robot_wrapper.h | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 462c1371..4c790e52 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -73,6 +73,52 @@ class ExampleRobotWrapper * \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. */ + [[deprecated("Deprecateing constructor for ExampleRobotWrapper. Please use constructor with both recipe files and " + "vectors")]] + 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. + */ + [[deprecated("Deprecateing constructor for ExampleRobotWrapper. Please use constructor with both recipe files and " + "vectors")]] + 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); + /*! + * \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_file Output recipe file for RTDE communication + * \param input_recipe_file Input recipe file for RTDE communication + * \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::string& output_recipe_file, const std::string& input_recipe_file, const std::vector output_recipe, const std::vector input_recipe, const bool headless_mode = true,