From 29f109ca1265660d8c528bf29bf768e4424c614a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 17:19:00 +0100 Subject: [PATCH 1/3] Use a config struct for initializing UrDriver --- include/ur_client_library/ur/ur_driver.h | 234 ++++++++++++++++-- src/example_robot_wrapper.cpp | 14 +- src/ur/ur_driver.cpp | 121 ++++----- tests/CMakeLists.txt | 6 + ...test_deprecated_ur_driver_construction.cpp | 109 ++++++++ 5 files changed, 378 insertions(+), 106 deletions(-) create mode 100644 tests/test_deprecated_ur_driver_construction.cpp diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 6a34d38b9..73048fc91 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -44,6 +44,81 @@ namespace urcl { +/*! + * \brief Structure for configuration parameters of a UrDriver object. + */ +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. + + /*! + * \brief Function handle to a callback on program state changes. + * + * For this to work, the URScript program will have to send keepalive signals to the \p + * reverse_port. + */ + std::function handle_program_state; + 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. + + /*! + * \brief Port that will be opened by the driver to allow direct communication between the driver + * and the robot controller. + */ + uint32_t reverse_port = 50001; + + /*! \brief The driver will offer an interface to receive the program's URScript on this port. If + * the robot cannot connect to this port, `External Control` will stop immediately. + */ + uint32_t script_sender_port = 50002; + + /*! + * \brief Port used for sending trajectory points to the robot in case of trajectory forwarding. + */ + uint32_t trajectory_port = 50003; + + /*! + * \brief Port used for forwarding script commands to the robot. + * + * This interface supports a set of predefined commands. + * The script commands will be executed locally on the robot. + */ + uint32_t script_command_port = 50004; + + /*! + * \brief IP address that the reverse_port will get bound to. + * + * If not specified, the IP address of the interface that is used for connecting to the robot's RTDE port will be + * used. + */ + std::string reverse_ip = ""; + + /*! + * \brief Proportional gain for arm joints following target position, range [100,2000] + */ + int servoj_gain = 2000; + + /*! + * \brief Time [S], range [0.03,0.2] smoothens servoj calls with this lookahead time + */ + double servoj_lookahead_time = 0.03; + + bool non_blocking_read = false; + + // TODO: Remove on 2027-05 + // The following parameters are considered deprecated and will be removed in May 2027. + /// @private + std::string calibration_checksum = ""; + /// @private + double force_mode_damping = 0.025; + /// @private + double force_mode_gain_scaling = 0.5; +}; + /*! * \brief This is the main class for interfacing the driver. * @@ -56,6 +131,25 @@ class UrDriver public: /*! * \brief Constructs a new UrDriver object. + * + * An RTDE connection to the robot will be established using the given recipe files. However, RTDE + * communication will not be started automatically, as this requires an external structure to read + * data from the RTDE client using the getDataPackage() method periodically. Once this is setup, + * please use the startRTDECommunication() method to actually start RTDE communication. + * + * \param config Configuration struct for the UrDriver. See it's documentation for details. + */ + explicit UrDriver(const UrDriverConfiguration& config) + { + init(config); + } + + /*! + * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * Upon initialization this class will check the calibration checksum reported from the robot and * compare it to a checksum given by the user. If the checksums don't match, the driver will output * an error message. This is critical if you want to do forward or inverse kinematics based on the @@ -94,15 +188,41 @@ class UrDriver * \param script_command_port Port used for forwarding script commands to the robot. The script commands will be * executed locally on the robot. */ + // Called sigA in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, - const uint32_t script_command_port = 50004); + const uint32_t script_command_port = 50004) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + init(config); + } /*! * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -129,28 +249,43 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ - [[deprecated( - "Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has " - "been deprecated. Force mode parameters should be specified with each activiation of force mode, and " - "can be set in the function call to start force mode.")]] UrDriver(const std::string& robot_ip, - const std::string& script_file, - const std::string& output_recipe_file, - const std::string& input_recipe_file, - std::function handle_program_state, - bool headless_mode, - std::unique_ptr tool_comm_setup, - const uint32_t reverse_port, - const uint32_t script_sender_port, - int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, - const std::string& reverse_ip, - const uint32_t trajectory_port, - const uint32_t script_command_port, - double force_mode_damping, - double force_mode_gain_scaling = 0.5); + // Called sigB in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] + UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, + const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, + std::unique_ptr tool_comm_setup, const uint32_t reverse_port, + const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, + const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port, + double force_mode_damping, double force_mode_gain_scaling = 0.5) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); + } /*! * \brief Constructs a new UrDriver object. + * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -178,16 +313,44 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ + // Called sigC in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004, - double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5); + double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5) + { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.calibration_checksum = calibration_checksum; + config.tool_comm_setup = std::move(tool_comm_setup); + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); + } /*! * \brief Constructs a new UrDriver object. * + * \deprecated Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const + * UrDriverConfiguration& config) instead. This function will be removed in May 2027. + * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot. * \param output_recipe_file Filename where the output recipe is stored in. @@ -214,6 +377,9 @@ class UrDriver * \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1] * \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series) */ + // Called sigD in tests + [[deprecated("Initializing a UrDriver object with an argument list is deprecated. Please use UrDriver(const " + "UrDriverConfiguration& config) instead. This function will be removed in May 2027.")]] UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, @@ -221,11 +387,26 @@ class UrDriver bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004, double force_mode_damping = 0.025, double force_mode_gain_scaling = 0.5) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip, trajectory_port, script_command_port, - force_mode_damping, force_mode_gain_scaling) { + UrDriverConfiguration config; + config.robot_ip = robot_ip; + config.script_file = script_file; + config.output_recipe_file = output_recipe_file; + config.input_recipe_file = input_recipe_file; + config.handle_program_state = handle_program_state; + config.headless_mode = headless_mode; + config.calibration_checksum = calibration_checksum; + config.reverse_port = reverse_port; + config.script_sender_port = script_sender_port; + config.servoj_gain = servoj_gain; + config.servoj_lookahead_time = servoj_lookahead_time; + config.non_blocking_read = non_blocking_read; + config.reverse_ip = reverse_ip; + config.trajectory_port = trajectory_port; + config.script_command_port = script_command_port; + config.force_mode_damping = force_mode_damping; + config.force_mode_gain_scaling = force_mode_gain_scaling; + init(config); } virtual ~UrDriver() = default; @@ -669,6 +850,7 @@ class UrDriver private: static std::string readScriptFile(const std::string& filename); + void init(const UrDriverConfiguration& config); /*! * \brief Reconnects the secondary stream used to send program to the robot. * @@ -709,4 +891,4 @@ class UrDriver VersionInformation robot_version_; }; } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED \ No newline at end of file +#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index 90da08cf2..5a372e813 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -53,11 +53,15 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: throw UrException("Could not initialize robot with dashboard"); } - std::unique_ptr tool_comm_setup; - ur_driver_ = - std::make_shared(robot_ip, script_file, output_recipe_file, input_recipe_file, - std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1), - headless_mode, std::move(tool_comm_setup)); + UrDriverConfiguration driver_config; + driver_config.robot_ip = robot_ip; + driver_config.script_file = script_file; + driver_config.output_recipe_file = output_recipe_file; + driver_config.input_recipe_file = input_recipe_file; + driver_config.handle_program_state = + std::bind(&ExampleRobotWrapper::handleRobotProgramState, this, std::placeholders::_1); + driver_config.headless_mode = headless_mode; + ur_driver_ = std::make_shared(driver_config); if (!headless_mode && !std::empty(autostart_program)) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 6e62c3150..808833b0b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -52,21 +52,19 @@ static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PO static const std::string FORCE_MODE_SET_DAMPING_REPLACE("{{FORCE_MODE_SET_DAMPING_REPLACE}}"); static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("{{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}"); -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, - const uint32_t script_command_port) - : servoj_gain_(servoj_gain) - , servoj_lookahead_time_(servoj_lookahead_time) - , handle_program_state_(handle_program_state) - , robot_ip_(robot_ip) +void UrDriver::init(const UrDriverConfiguration& config) { + robot_ip_ = config.robot_ip; + non_blocking_read_ = config.non_blocking_read; + servoj_gain_ = config.servoj_gain; + servoj_lookahead_time_ = config.servoj_lookahead_time; + handle_program_state_ = config.handle_program_state; + in_headless_mode_ = config.headless_mode; + URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); - rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); + rtde_client_.reset( + new rtde_interface::RTDEClient(robot_ip_, notifier_, config.output_recipe_file, config.input_recipe_file)); primary_stream_.reset( new comm::URStream(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT)); @@ -76,16 +74,15 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); - non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; initRTDE(); - setupReverseInterface(reverse_port); + setupReverseInterface(config.reverse_port); // Figure out the ip automatically if the user didn't provide it - std::string local_ip = reverse_ip.empty() ? rtde_client_->getIP() : reverse_ip; + std::string local_ip = config.reverse_ip.empty() ? rtde_client_->getIP() : config.reverse_ip; - std::string prog = readScriptFile(script_file); + std::string prog = readScriptFile(config.script_file); while (prog.find(JOINT_STATE_REPLACE) != std::string::npos) { prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), @@ -111,24 +108,25 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ while (prog.find(SERVER_PORT_REPLACE) != std::string::npos) { - prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); + prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(config.reverse_port)); } while (prog.find(TRAJECTORY_PORT_REPLACE) != std::string::npos) { - prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(), std::to_string(trajectory_port)); + prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(), + std::to_string(config.trajectory_port)); } while (prog.find(SCRIPT_COMMAND_PORT_REPLACE) != std::string::npos) { prog.replace(prog.find(SCRIPT_COMMAND_PORT_REPLACE), SCRIPT_COMMAND_PORT_REPLACE.length(), - std::to_string(script_command_port)); + std::to_string(config.script_command_port)); } robot_version_ = rtde_client_->getVersion(); std::stringstream begin_replace; - if (tool_comm_setup != nullptr) + if (config.tool_comm_setup != nullptr) { if (robot_version_.major < 5) { @@ -137,15 +135,15 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ 5, robot_version_.major); } begin_replace << "set_tool_voltage(" - << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; - begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", " - << static_cast::type>(tool_comm_setup->getParity()) << ", " - << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " - << tool_comm_setup->getTxIdleChars() << ")"; + << static_cast::type>(config.tool_comm_setup->getToolVoltage()) + << ")\n"; + begin_replace << "set_tool_communication(" << "True" << ", " << config.tool_comm_setup->getBaudRate() << ", " + << static_cast::type>(config.tool_comm_setup->getParity()) << ", " + << config.tool_comm_setup->getStopBits() << ", " << config.tool_comm_setup->getRxIdleChars() << ", " + << config.tool_comm_setup->getTxIdleChars() << ")"; } prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str()); - in_headless_mode_ = headless_mode; if (in_headless_mode_) { full_robot_program_ = "stop program\n"; @@ -161,61 +159,34 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ } else { - script_sender_.reset(new control::ScriptSender(script_sender_port, prog)); + script_sender_.reset(new control::ScriptSender(config.script_sender_port, prog)); URCL_LOG_DEBUG("Created script sender"); } - trajectory_interface_.reset(new control::TrajectoryPointInterface(trajectory_port)); - script_command_interface_.reset(new control::ScriptCommandInterface(script_command_port)); - - URCL_LOG_DEBUG("Initialization done"); -} + trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port)); + script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port)); -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, - const uint32_t script_command_port, double force_mode_damping, double force_mode_gain_scaling) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port, script_command_port) -{ - force_mode_damping_factor_ = force_mode_damping; - force_mode_gain_scale_factor_ = force_mode_gain_scaling; -} - -urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& output_recipe_file, const std::string& input_recipe_file, - std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, - const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, - double servoj_lookahead_time, bool non_blocking_read, const std::string& reverse_ip, - const uint32_t trajectory_port, const uint32_t script_command_port, double force_mode_damping, - double force_mode_gain_scaling) - : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port, script_command_port, force_mode_damping, - force_mode_gain_scaling) -{ - 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 " - "this library, you can ignore this message."); - if (checkCalibration(calibration_checksum)) - { - URCL_LOG_INFO("Calibration checked successfully."); - } - else + if (!std::empty(config.calibration_checksum)) { - 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] " - "for details."); + 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 " + "this library, you can ignore this message."); + if (checkCalibration(config.calibration_checksum)) + { + URCL_LOG_INFO("Calibration checked successfully."); + } + 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 " + "description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " + "for details."); + } } + URCL_LOG_DEBUG("Initialization done"); } std::unique_ptr urcl::UrDriver::getDataPackage() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 890092809..e4eda80a1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -94,6 +94,12 @@ if (INTEGRATION_TESTS) EXTRA_ARGS --headless true TEST_SUFFIX _headless ) + + add_executable(ur_driver_deprecated_constructor_test test_deprecated_ur_driver_construction.cpp) + target_link_libraries(ur_driver_deprecated_constructor_test PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET ur_driver_deprecated_constructor_test + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + ) else() message(STATUS "Skipping integration tests.") endif() diff --git a/tests/test_deprecated_ur_driver_construction.cpp b/tests/test_deprecated_ur_driver_construction.cpp new file mode 100644 index 000000000..08b55e65f --- /dev/null +++ b/tests/test_deprecated_ur_driver_construction.cpp @@ -0,0 +1,109 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include "ur_client_library/ur/ur_driver.h" + +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::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; +std::string g_ROBOT_IP = "192.168.56.101"; +bool g_HEADLESS = true; + +void handleRobotProgramState(bool program_running) +{ + std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; +} + +TEST(UrDriverTestDeprecatedConstructor, sigA) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup)); + driver->checkCalibration(CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigB) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared( + g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, std::bind(&handleRobotProgramState, std::placeholders::_1), + g_HEADLESS, std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003, 50004, 0.025, 0.5); + driver->checkCalibration(CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigC) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup), CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +TEST(UrDriverTestDeprecatedConstructor, sigD) +{ + std::unique_ptr tool_comm_setup; + auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + CALIBRATION_CHECKSUM); + auto version = driver->getVersion(); + ASSERT_TRUE(version.major > 0); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + for (int i = 0; i < argc; i++) + { + if (std::string(argv[i]) == "--robot_ip" && i + 1 < argc) + { + g_ROBOT_IP = argv[i + 1]; + break; + } + if (std::string(argv[i]) == "--headless" && i + 1 < argc) + { + std::string headless = argv[i + 1]; + g_HEADLESS = headless == "true" || headless == "1" || headless == "True" || headless == "TRUE"; + break; + } + } + + return RUN_ALL_TESTS(); +} From d15466ee08c70991e33953bb0d6e385812cde3d7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 17:34:25 +0100 Subject: [PATCH 2/3] Disable internal deprecation warning We forward the deprecated setKeepalive method through the UrDriver which triggers a depracation warning. We disable it (for GCC) for this call. --- src/ur/ur_driver.cpp | 4 ++++ tests/test_reverse_interface.cpp | 3 +++ 2 files changed, 7 insertions(+) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 808833b0b..f200b0308 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -665,7 +665,11 @@ void UrDriver::setKeepaliveCount(const uint32_t count) "set the " "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 +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" reverse_interface_->setKeepaliveCount(count); +#pragma GCC diagnostic pop } void UrDriver::resetRTDEClient(const std::vector& output_recipe, diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 0d356c4b1..73466cfde 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -428,7 +428,10 @@ TEST_F(ReverseIntefaceTest, deprecated_set_keep_alive_count) // Test that it works to set the keepalive count using the deprecated function int keep_alive_count = 10; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" reverse_interface_->setKeepaliveCount(keep_alive_count); +#pragma GCC diagnostic pop int32_t expected_read_timeout = 20 * keep_alive_count; urcl::vector6d_t pos = { 0, 0, 0, 0, 0, 0 }; From 2ce3c9b6cc9a3564fb4c5876ea3d75579afcdc7b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 11 Feb 2025 10:06:27 +0100 Subject: [PATCH 3/3] Increase dashboard timeout in ExampleRobotWrapper to 10s --- src/example_robot_wrapper.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index 5a372e813..8f405c5d1 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -48,6 +48,13 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: URCL_LOG_ERROR("Could not connect to dashboard"); } + // In CI we the dashboard client times out for no obvious reason. Hence we increase the timeout + // here. + timeval tv; + tv.tv_sec = 10; + tv.tv_usec = 0; + dashboard_client_->setReceiveTimeout(tv); + if (!initializeRobotWithDashboard()) { throw UrException("Could not initialize robot with dashboard");