diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 7756aca47..afd9100bf 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -43,6 +43,8 @@ template class IConsumer { public: + virtual ~IConsumer() = default; + /*! * \brief Set-up functionality of the consumer. */ @@ -170,6 +172,8 @@ template class IProducer { public: + virtual ~IProducer() = default; + /*! * \brief Set-up functionality of the producers. * diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 1035f6e40..17dfd0fae 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -29,6 +29,8 @@ #ifndef UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED #define UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED +#include + #include "ur_client_library/comm/pipeline.h" #include "ur_client_library/rtde/package_header.h" #include "ur_client_library/rtde/rtde_package.h" @@ -102,9 +104,12 @@ class RTDEClient * \param output_recipe_file Path to the file containing the output recipe * \param input_recipe_file Path to the file containing the input recipe * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency. + * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, - const std::string& input_recipe_file, double target_frequency = 0.0); + const std::string& input_recipe_file, double target_frequency = 0.0, + bool ignore_unavailable_outputs = false); /*! * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the @@ -115,9 +120,12 @@ class RTDEClient * \param output_recipe Vector containing the output recipe * \param input_recipe Vector containing the input recipe * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency. + * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector& output_recipe, - const std::vector& input_recipe, double target_frequency = 0.0); + const std::vector& input_recipe, double target_frequency = 0.0, + bool ignore_unavailable_outputs = false); ~RTDEClient(); /*! * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the @@ -210,10 +218,12 @@ class RTDEClient private: comm::URStream stream_; std::vector output_recipe_; + bool ignore_unavailable_outputs_; std::vector input_recipe_; RTDEParser parser_; - comm::URProducer prod_; - comm::Pipeline pipeline_; + std::unique_ptr> prod_; + comm::INotifier notifier_; + std::unique_ptr> pipeline_; RTDEWriter writer_; VersionInformation urcontrol_version_; @@ -241,6 +251,14 @@ class RTDEClient void setupInputs(); void disconnect(); + /*! + * \brief Updates the output recipe to the given one and recreates all the objects which depend on it. + * It should only be called while setting up the communication. + * + * \param new_recipe the new output recipe to use + */ + void resetOutputRecipe(const std::vector new_recipe); + /*! * \brief Checks whether the robot is booted, this is done by looking at the timestamp from the robot controller, this * will show the time in seconds since the controller was started. If the timestamp is below 40, we will read from diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index cf57963e3..358e1b58e 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -605,10 +605,13 @@ class UrDriver * * \param output_recipe Vector containing the output recipe * \param input_recipe Vector containing the input recipe - * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency. + * \param target_frequency + * Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency. + * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ void resetRTDEClient(const std::vector& output_recipe, const std::vector& input_recipe, - double target_frequency = 0.0); + double target_frequency = 0.0, bool ignore_unavailable_outputs = false); /** * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication @@ -621,9 +624,11 @@ class UrDriver * \param output_recipe_filename Filename where the output recipe is stored in. * \param input_recipe_filename Filename where the input recipe is stored in. * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency. + * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, - double target_frequency = 0.0); + double target_frequency = 0.0, bool ignore_unavailable_outputs = false); private: static std::string readScriptFile(const std::string& filename); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index f31587988..2e05fad4f 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -35,13 +35,15 @@ namespace urcl namespace rtde_interface { RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, - const std::string& input_recipe_file, double target_frequency) + const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs) : stream_(robot_ip, UR_RTDE_PORT) , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file))) + , ignore_unavailable_outputs_(ignore_unavailable_outputs) , input_recipe_(readRecipe(input_recipe_file)) , parser_(output_recipe_) - , prod_(stream_, parser_) - , pipeline_(prod_, PIPELINE_NAME, notifier, true) + , prod_(std::make_unique>(stream_, parser_)) + , notifier_(notifier) + , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) @@ -50,13 +52,16 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st } RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector& output_recipe, - const std::vector& input_recipe, double target_frequency) + const std::vector& input_recipe, double target_frequency, + bool ignore_unavailable_outputs) : stream_(robot_ip, UR_RTDE_PORT) , output_recipe_(ensureTimestampIsPresent(output_recipe)) + , ignore_unavailable_outputs_(ignore_unavailable_outputs) , input_recipe_(input_recipe) , parser_(output_recipe_) - , prod_(stream_, parser_) - , pipeline_(prod_, PIPELINE_NAME, notifier, true) + , prod_(std::make_unique>(stream_, parser_)) + , notifier_(notifier) + , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) @@ -96,8 +101,8 @@ void RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron { client_state_ = ClientState::INITIALIZING; // A running pipeline is needed inside setup - pipeline_.init(max_num_tries, reconnection_time); - pipeline_.run(); + pipeline_->init(max_num_tries, reconnection_time); + pipeline_->run(); uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION; while (!negotiateProtocolVersion(protocol_version) && client_state_ == ClientState::INITIALIZING) @@ -151,7 +156,7 @@ void RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron return; // We finished communication for now - pipeline_.stop(); + pipeline_->stop(); client_state_ = ClientState::INITIALIZED; } @@ -174,7 +179,7 @@ bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) while (num_retries < MAX_REQUEST_RETRIES) { std::unique_ptr package; - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("failed to get package from rtde interface, disconnecting"); disconnect(); @@ -220,7 +225,7 @@ void RTDEClient::queryURControlVersion() std::unique_ptr package; while (num_retries < MAX_REQUEST_RETRIES) { - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("No answer to urcontrol version query was received from robot, disconnecting"); disconnect(); @@ -249,6 +254,17 @@ void RTDEClient::queryURControlVersion() throw UrException(ss.str()); } +void RTDEClient::resetOutputRecipe(const std::vector new_recipe) +{ + prod_->teardownProducer(); + disconnect(); + + output_recipe_.assign(new_recipe.begin(), new_recipe.end()); + parser_ = RTDEParser(output_recipe_); + prod_ = std::make_unique>(stream_, parser_); + pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true); +} + void RTDEClient::setupOutputs(const uint16_t protocol_version) { unsigned int num_retries = 0; @@ -256,32 +272,34 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) size_t written; uint8_t buffer[8192]; URCL_LOG_INFO("Setting up RTDE communication with frequency %f", target_frequency_); - if (protocol_version == 2) - { - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_); - } - else + + while (num_retries < MAX_REQUEST_RETRIES) { - if (target_frequency_ != max_frequency_) + URCL_LOG_DEBUG("Sending output recipe"); + if (protocol_version == 2) { - URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency " - "equivalent to the maximum frequency will be used instead."); + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_); + } + else + { + if (target_frequency_ != max_frequency_) + { + URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency " + "equivalent to the maximum frequency will be used instead."); + } + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_); } - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_); - } - // Send output recipe to robot - if (!stream_.write(buffer, size, written)) - { - URCL_LOG_ERROR("Could not send RTDE output recipe to robot, disconnecting"); - disconnect(); - return; - } + // Send output recipe to robot + if (!stream_.write(buffer, size, written)) + { + URCL_LOG_ERROR("Could not send RTDE output recipe to robot, disconnecting"); + disconnect(); + return; + } - while (num_retries < MAX_REQUEST_RETRIES) - { std::unique_ptr package; - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe, disconnecting"); disconnect(); @@ -293,18 +311,53 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) { std::vector variable_types = splitVariableTypes(tmp_output->variable_types_); + std::vector available_variables; + std::vector unavailable_variables; assert(output_recipe_.size() == variable_types.size()); for (std::size_t i = 0; i < variable_types.size(); ++i) { - URCL_LOG_DEBUG("%s confirmed as datatype: %s", output_recipe_[i].c_str(), variable_types[i].c_str()); + const std::string variable_name = output_recipe_[i]; + URCL_LOG_DEBUG("%s confirmed as datatype: %s", variable_name.c_str(), variable_types[i].c_str()); + if (variable_types[i] == "NOT_FOUND") { - std::string message = "Variable '" + output_recipe_[i] + - "' not recognized by the robot. Probably your output recipe contains errors"; - throw UrException(message); + unavailable_variables.push_back(variable_name); + } + else + { + available_variables.push_back(variable_name); } } - return; + + if (!unavailable_variables.empty()) + { + std::stringstream error_message; + error_message << "The following variables are not recognized by the robot: "; + std::for_each(unavailable_variables.begin(), unavailable_variables.end(), + [&error_message](const std::string& variable_name) { error_message << variable_name << " "; }); + error_message << ". Either your output recipe contains errors " + "or the urcontrol version does not support " + "them."; + + if (ignore_unavailable_outputs_) + { + error_message << " They will be removed from the output recipe."; + URCL_LOG_WARN("%s", error_message.str().c_str()); + + // Some variables are not available so retry setting up the communication with a stripped-down output recipe + resetOutputRecipe(available_variables); + } + else + { + URCL_LOG_ERROR("%s", error_message.str().c_str()); + throw UrException(error_message.str()); + } + } + else + { + // All variables are accounted for in the RTDE package + return; + } } else { @@ -339,7 +392,7 @@ void RTDEClient::setupInputs() while (num_retries < MAX_REQUEST_RETRIES) { std::unique_ptr package; - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe, disconnecting"); disconnect(); @@ -395,7 +448,7 @@ void RTDEClient::disconnect() if (client_state_ > ClientState::UNINITIALIZED) { sendPause(); - pipeline_.stop(); + pipeline_->stop(); stream_.disconnect(); } client_state_ = ClientState::UNINITIALIZED; @@ -421,7 +474,7 @@ bool RTDEClient::isRobotBooted() { // Set timeout based on target frequency, to make sure that reading doesn't timeout int timeout = static_cast((1 / target_frequency_) * 1000) * 10; - if (pipeline_.getLatestProduct(package, std::chrono::milliseconds(timeout))) + if (pipeline_->getLatestProduct(package, std::chrono::milliseconds(timeout))) { rtde_interface::DataPackage* tmp_input = dynamic_cast(package.get()); tmp_input->getData("timestamp", timestamp); @@ -451,7 +504,7 @@ bool RTDEClient::start() return false; } - pipeline_.run(); + pipeline_->run(); if (sendStart()) { @@ -501,7 +554,7 @@ bool RTDEClient::sendStart() unsigned int num_retries = 0; while (num_retries < MAX_REQUEST_RETRIES) { - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("Could not get response to RTDE communication start request from robot"); return false; @@ -543,7 +596,7 @@ bool RTDEClient::sendPause() int seconds = 5; while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) { - if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot"); return false; @@ -605,7 +658,7 @@ std::vector RTDEClient::ensureTimestampIsPresent(const std::vector< std::unique_ptr RTDEClient::getDataPackage(std::chrono::milliseconds timeout) { std::unique_ptr urpackage; - if (pipeline_.getLatestProduct(urpackage, timeout)) + if (pipeline_->getLatestProduct(urpackage, timeout)) { rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get()); if (tmp != nullptr) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index c18cfa1f2..3dc128751 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -691,18 +691,19 @@ void UrDriver::setKeepaliveCount(const uint32_t count) } void UrDriver::resetRTDEClient(const std::vector& output_recipe, - const std::vector& input_recipe, double target_frequency) + const std::vector& input_recipe, double target_frequency, + bool ignore_unavailable_outputs) { - rtde_client_.reset( - new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe, target_frequency)); + rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe, target_frequency, + ignore_unavailable_outputs)); initRTDE(); } void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, - double target_frequency) + double target_frequency, bool ignore_unavailable_outputs) { rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_filename, input_recipe_filename, - target_frequency)); + target_frequency, ignore_unavailable_outputs)); initRTDE(); } diff --git a/tests/resources/exhaustive_rtde_output_recipe.txt b/tests/resources/exhaustive_rtde_output_recipe.txt new file mode 100644 index 000000000..0d87fc963 --- /dev/null +++ b/tests/resources/exhaustive_rtde_output_recipe.txt @@ -0,0 +1,342 @@ +timestamp +target_q +target_qd +target_qdd +target_current +target_moment +actual_q +actual_qd +actual_current +actual_current_window +joint_control_output +actual_TCP_pose +actual_TCP_speed +actual_TCP_force +target_TCP_pose +target_TCP_speed +actual_digital_input_bits +joint_temperatures +actual_execution_time +robot_mode +joint_mode +safety_mode +safety_status +actual_tool_accelerometer +speed_scaling +target_speed_fraction +actual_momentum +actual_main_voltage +actual_robot_voltage +actual_robot_current +actual_joint_voltage +actual_digital_output_bits +runtime_state +elbow_position +elbow_velocity +robot_status_bits +safety_status_bits +analog_io_types +standard_analog_input0 +standard_analog_input1 +standard_analog_output0 +standard_analog_output1 +io_current +euromap67_input_bits +euromap67_output_bits +euromap67_24V_voltage +euromap67_24V_current +tool_mode +tool_analog_input_types +tool_analog_input0 +tool_analog_input1 +tool_output_voltage +tool_output_current +tool_temperature +tcp_force_scalar +output_bit_registers0_to_31 +output_bit_registers32_to_63 +output_bit_register_64 +output_bit_register_65 +output_bit_register_66 +output_bit_register_67 +output_bit_register_68 +output_bit_register_69 +output_bit_register_70 +output_bit_register_71 +output_bit_register_72 +output_bit_register_73 +output_bit_register_74 +output_bit_register_75 +output_bit_register_76 +output_bit_register_77 +output_bit_register_78 +output_bit_register_79 +output_bit_register_80 +output_bit_register_81 +output_bit_register_82 +output_bit_register_83 +output_bit_register_84 +output_bit_register_85 +output_bit_register_86 +output_bit_register_87 +output_bit_register_88 +output_bit_register_89 +output_bit_register_90 +output_bit_register_91 +output_bit_register_92 +output_bit_register_93 +output_bit_register_94 +output_bit_register_95 +output_bit_register_96 +output_bit_register_97 +output_bit_register_98 +output_bit_register_99 +output_bit_register_100 +output_bit_register_101 +output_bit_register_102 +output_bit_register_103 +output_bit_register_104 +output_bit_register_105 +output_bit_register_106 +output_bit_register_107 +output_bit_register_108 +output_bit_register_109 +output_bit_register_110 +output_bit_register_111 +output_bit_register_112 +output_bit_register_113 +output_bit_register_114 +output_bit_register_115 +output_bit_register_116 +output_bit_register_117 +output_bit_register_118 +output_bit_register_119 +output_bit_register_120 +output_bit_register_121 +output_bit_register_122 +output_bit_register_123 +output_bit_register_124 +output_bit_register_125 +output_bit_register_126 +output_bit_register_127 +output_int_register_0 +output_int_register_1 +output_int_register_2 +output_int_register_3 +output_int_register_4 +output_int_register_5 +output_int_register_6 +output_int_register_7 +output_int_register_8 +output_int_register_9 +output_int_register_10 +output_int_register_11 +output_int_register_12 +output_int_register_13 +output_int_register_14 +output_int_register_15 +output_int_register_16 +output_int_register_17 +output_int_register_18 +output_int_register_19 +output_int_register_20 +output_int_register_21 +output_int_register_22 +output_int_register_23 +output_int_register_24 +output_int_register_25 +output_int_register_26 +output_int_register_27 +output_int_register_28 +output_int_register_29 +output_int_register_30 +output_int_register_31 +output_int_register_32 +output_int_register_33 +output_int_register_34 +output_int_register_35 +output_int_register_36 +output_int_register_37 +output_int_register_38 +output_int_register_39 +output_int_register_40 +output_int_register_41 +output_int_register_42 +output_int_register_43 +output_int_register_44 +output_int_register_45 +output_int_register_46 +output_int_register_47 +output_double_register_0 +output_double_register_1 +output_double_register_2 +output_double_register_3 +output_double_register_4 +output_double_register_5 +output_double_register_6 +output_double_register_7 +output_double_register_8 +output_double_register_9 +output_double_register_10 +output_double_register_11 +output_double_register_12 +output_double_register_13 +output_double_register_14 +output_double_register_15 +output_double_register_16 +output_double_register_17 +output_double_register_18 +output_double_register_19 +output_double_register_20 +output_double_register_21 +output_double_register_22 +output_double_register_23 +output_double_register_24 +output_double_register_25 +output_double_register_26 +output_double_register_27 +output_double_register_28 +output_double_register_29 +output_double_register_30 +output_double_register_31 +output_double_register_32 +output_double_register_33 +output_double_register_34 +output_double_register_35 +output_double_register_36 +output_double_register_37 +output_double_register_38 +output_double_register_39 +output_double_register_40 +output_double_register_41 +output_double_register_42 +output_double_register_43 +output_double_register_44 +output_double_register_45 +output_double_register_46 +output_double_register_47 +input_bit_registers0_to_31 +input_bit_registers32_to_63 +input_bit_register_64 +input_bit_register_65 +input_bit_register_66 +input_bit_register_67 +input_bit_register_68 +input_bit_register_69 +input_bit_register_70 +input_bit_register_71 +input_bit_register_72 +input_bit_register_73 +input_bit_register_74 +input_bit_register_75 +input_bit_register_76 +input_bit_register_77 +input_bit_register_78 +input_bit_register_79 +input_bit_register_80 +input_bit_register_81 +input_bit_register_82 +input_bit_register_83 +input_bit_register_84 +input_bit_register_85 +input_bit_register_86 +input_bit_register_87 +input_bit_register_88 +input_bit_register_89 +input_bit_register_90 +input_bit_register_91 +input_bit_register_92 +input_bit_register_93 +input_bit_register_94 +input_bit_register_95 +input_bit_register_96 +input_bit_register_97 +input_bit_register_98 +input_bit_register_99 +input_bit_register_100 +input_bit_register_101 +input_bit_register_102 +input_bit_register_103 +input_bit_register_104 +input_bit_register_105 +input_bit_register_106 +input_bit_register_107 +input_bit_register_108 +input_bit_register_109 +input_bit_register_110 +input_bit_register_111 +input_bit_register_112 +input_bit_register_113 +input_bit_register_114 +input_bit_register_115 +input_bit_register_116 +input_bit_register_117 +input_bit_register_118 +input_bit_register_119 +input_bit_register_120 +input_bit_register_121 +input_bit_register_122 +input_bit_register_123 +input_bit_register_124 +input_bit_register_125 +input_bit_register_126 +input_bit_register_127 +input_double_register_0 +input_double_register_1 +input_double_register_2 +input_double_register_3 +input_double_register_4 +input_double_register_5 +input_double_register_6 +input_double_register_7 +input_double_register_8 +input_double_register_9 +input_double_register_10 +input_double_register_11 +input_double_register_12 +input_double_register_13 +input_double_register_14 +input_double_register_15 +input_double_register_16 +input_double_register_17 +input_double_register_18 +input_double_register_19 +input_double_register_20 +input_double_register_21 +input_double_register_22 +input_double_register_23 +input_double_register_24 +input_double_register_25 +input_double_register_26 +input_double_register_27 +input_double_register_28 +input_double_register_29 +input_double_register_30 +input_double_register_31 +input_double_register_32 +input_double_register_33 +input_double_register_34 +input_double_register_35 +input_double_register_36 +input_double_register_37 +input_double_register_38 +input_double_register_39 +input_double_register_40 +input_double_register_41 +input_double_register_42 +input_double_register_43 +input_double_register_44 +input_double_register_45 +input_double_register_46 +input_double_register_47 +tool_output_mode +tool_digital_output0_mode +tool_digital_output1_mode +payload +payload_cog +payload_inertia +script_control_line +ft_raw_wrench +joint_position_deviation_ratio +collision_detection_ratio +time_scale_source diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 8158dac9d..d72011d10 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -42,355 +42,6 @@ using namespace urcl; std::string ROBOT_IP = "192.168.56.101"; -// Based on https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/ -using MinCbSeriesVersion = std::optional; -using MinESeriesVersion = std::optional; -using OutputNamesWithCompatibility = std::unordered_map>; -const OutputNamesWithCompatibility EXHAUSTIVE_OUTPUTS_WITH_COMPATIBILITY = { - { "timestamp", { std::nullopt, std::nullopt } }, - { "target_q", { std::nullopt, std::nullopt } }, - { "target_qd", { std::nullopt, std::nullopt } }, - { "target_qdd", { std::nullopt, std::nullopt } }, - { "target_current", { std::nullopt, std::nullopt } }, - { "target_moment", { std::nullopt, std::nullopt } }, - { "actual_q", { std::nullopt, std::nullopt } }, - { "actual_qd", { std::nullopt, std::nullopt } }, - { "actual_current", { std::nullopt, std::nullopt } }, - { "actual_current_window", { std::nullopt, std::nullopt } }, - { "joint_control_output", { std::nullopt, std::nullopt } }, - { "actual_TCP_pose", { std::nullopt, std::nullopt } }, - { "actual_TCP_speed", { std::nullopt, std::nullopt } }, - { "actual_TCP_force", { std::nullopt, std::nullopt } }, - { "target_TCP_pose", { std::nullopt, std::nullopt } }, - { "target_TCP_speed", { std::nullopt, std::nullopt } }, - { "actual_digital_input_bits", { std::nullopt, std::nullopt } }, - { "joint_temperatures", { std::nullopt, std::nullopt } }, - { "actual_execution_time", { std::nullopt, std::nullopt } }, - { "robot_mode", { std::nullopt, std::nullopt } }, - { "joint_mode", { std::nullopt, std::nullopt } }, - { "safety_mode", { std::nullopt, std::nullopt } }, - { "safety_status", { "3.10.0", "5.4.0" } }, - { "actual_tool_accelerometer", { std::nullopt, std::nullopt } }, - { "speed_scaling", { std::nullopt, std::nullopt } }, - { "target_speed_fraction", { std::nullopt, std::nullopt } }, - { "actual_momentum", { std::nullopt, std::nullopt } }, - { "actual_main_voltage", { std::nullopt, std::nullopt } }, - { "actual_robot_voltage", { std::nullopt, std::nullopt } }, - { "actual_robot_current", { std::nullopt, std::nullopt } }, - { "actual_joint_voltage", { std::nullopt, std::nullopt } }, - { "actual_digital_output_bits", { std::nullopt, std::nullopt } }, - { "runtime_state", { std::nullopt, std::nullopt } }, - { "elbow_position", { "3.5.0", "5.0.0" } }, - { "elbow_velocity", { "3.5.0", "5.0.0" } }, - { "robot_status_bits", { std::nullopt, std::nullopt } }, - { "safety_status_bits", { std::nullopt, std::nullopt } }, - { "analog_io_types", { std::nullopt, std::nullopt } }, - { "standard_analog_input0", { std::nullopt, std::nullopt } }, - { "standard_analog_input1", { std::nullopt, std::nullopt } }, - { "standard_analog_output0", { std::nullopt, std::nullopt } }, - { "standard_analog_output1", { std::nullopt, std::nullopt } }, - { "io_current", { std::nullopt, std::nullopt } }, - { "euromap67_input_bits", { std::nullopt, std::nullopt } }, - { "euromap67_output_bits", { std::nullopt, std::nullopt } }, - { "euromap67_24V_voltage", { std::nullopt, std::nullopt } }, - { "euromap67_24V_current", { std::nullopt, std::nullopt } }, - { "tool_mode", { std::nullopt, std::nullopt } }, - { "tool_analog_input_types", { std::nullopt, std::nullopt } }, - { "tool_analog_input0", { std::nullopt, std::nullopt } }, - { "tool_analog_input1", { std::nullopt, std::nullopt } }, - { "tool_output_voltage", { std::nullopt, std::nullopt } }, - { "tool_output_current", { std::nullopt, std::nullopt } }, - { "tool_temperature", { std::nullopt, std::nullopt } }, - { "tcp_force_scalar", { std::nullopt, std::nullopt } }, - { "output_bit_registers0_to_31", { std::nullopt, std::nullopt } }, - { "output_bit_registers32_to_63", { std::nullopt, std::nullopt } }, - { "output_bit_register_64", { "3.9.0", "5.3.0" } }, - { "output_bit_register_65", { "3.9.0", "5.3.0" } }, - { "output_bit_register_66", { "3.9.0", "5.3.0" } }, - { "output_bit_register_67", { "3.9.0", "5.3.0" } }, - { "output_bit_register_68", { "3.9.0", "5.3.0" } }, - { "output_bit_register_69", { "3.9.0", "5.3.0" } }, - { "output_bit_register_70", { "3.9.0", "5.3.0" } }, - { "output_bit_register_71", { "3.9.0", "5.3.0" } }, - { "output_bit_register_72", { "3.9.0", "5.3.0" } }, - { "output_bit_register_73", { "3.9.0", "5.3.0" } }, - { "output_bit_register_74", { "3.9.0", "5.3.0" } }, - { "output_bit_register_75", { "3.9.0", "5.3.0" } }, - { "output_bit_register_76", { "3.9.0", "5.3.0" } }, - { "output_bit_register_77", { "3.9.0", "5.3.0" } }, - { "output_bit_register_78", { "3.9.0", "5.3.0" } }, - { "output_bit_register_79", { "3.9.0", "5.3.0" } }, - { "output_bit_register_80", { "3.9.0", "5.3.0" } }, - { "output_bit_register_81", { "3.9.0", "5.3.0" } }, - { "output_bit_register_82", { "3.9.0", "5.3.0" } }, - { "output_bit_register_83", { "3.9.0", "5.3.0" } }, - { "output_bit_register_84", { "3.9.0", "5.3.0" } }, - { "output_bit_register_85", { "3.9.0", "5.3.0" } }, - { "output_bit_register_86", { "3.9.0", "5.3.0" } }, - { "output_bit_register_87", { "3.9.0", "5.3.0" } }, - { "output_bit_register_88", { "3.9.0", "5.3.0" } }, - { "output_bit_register_89", { "3.9.0", "5.3.0" } }, - { "output_bit_register_90", { "3.9.0", "5.3.0" } }, - { "output_bit_register_91", { "3.9.0", "5.3.0" } }, - { "output_bit_register_92", { "3.9.0", "5.3.0" } }, - { "output_bit_register_93", { "3.9.0", "5.3.0" } }, - { "output_bit_register_94", { "3.9.0", "5.3.0" } }, - { "output_bit_register_95", { "3.9.0", "5.3.0" } }, - { "output_bit_register_96", { "3.9.0", "5.3.0" } }, - { "output_bit_register_97", { "3.9.0", "5.3.0" } }, - { "output_bit_register_98", { "3.9.0", "5.3.0" } }, - { "output_bit_register_99", { "3.9.0", "5.3.0" } }, - { "output_bit_register_100", { "3.9.0", "5.3.0" } }, - { "output_bit_register_101", { "3.9.0", "5.3.0" } }, - { "output_bit_register_102", { "3.9.0", "5.3.0" } }, - { "output_bit_register_103", { "3.9.0", "5.3.0" } }, - { "output_bit_register_104", { "3.9.0", "5.3.0" } }, - { "output_bit_register_105", { "3.9.0", "5.3.0" } }, - { "output_bit_register_106", { "3.9.0", "5.3.0" } }, - { "output_bit_register_107", { "3.9.0", "5.3.0" } }, - { "output_bit_register_108", { "3.9.0", "5.3.0" } }, - { "output_bit_register_109", { "3.9.0", "5.3.0" } }, - { "output_bit_register_110", { "3.9.0", "5.3.0" } }, - { "output_bit_register_111", { "3.9.0", "5.3.0" } }, - { "output_bit_register_112", { "3.9.0", "5.3.0" } }, - { "output_bit_register_113", { "3.9.0", "5.3.0" } }, - { "output_bit_register_114", { "3.9.0", "5.3.0" } }, - { "output_bit_register_115", { "3.9.0", "5.3.0" } }, - { "output_bit_register_116", { "3.9.0", "5.3.0" } }, - { "output_bit_register_117", { "3.9.0", "5.3.0" } }, - { "output_bit_register_118", { "3.9.0", "5.3.0" } }, - { "output_bit_register_119", { "3.9.0", "5.3.0" } }, - { "output_bit_register_120", { "3.9.0", "5.3.0" } }, - { "output_bit_register_121", { "3.9.0", "5.3.0" } }, - { "output_bit_register_122", { "3.9.0", "5.3.0" } }, - { "output_bit_register_123", { "3.9.0", "5.3.0" } }, - { "output_bit_register_124", { "3.9.0", "5.3.0" } }, - { "output_bit_register_125", { "3.9.0", "5.3.0" } }, - { "output_bit_register_126", { "3.9.0", "5.3.0" } }, - { "output_bit_register_127", { "3.9.0", "5.3.0" } }, - { "output_int_register_0", { "3.4.0", std::nullopt } }, - { "output_int_register_1", { "3.4.0", std::nullopt } }, - { "output_int_register_2", { "3.4.0", std::nullopt } }, - { "output_int_register_3", { "3.4.0", std::nullopt } }, - { "output_int_register_4", { "3.4.0", std::nullopt } }, - { "output_int_register_5", { "3.4.0", std::nullopt } }, - { "output_int_register_6", { "3.4.0", std::nullopt } }, - { "output_int_register_7", { "3.4.0", std::nullopt } }, - { "output_int_register_8", { "3.4.0", std::nullopt } }, - { "output_int_register_9", { "3.4.0", std::nullopt } }, - { "output_int_register_10", { "3.4.0", std::nullopt } }, - { "output_int_register_11", { "3.4.0", std::nullopt } }, - { "output_int_register_12", { "3.4.0", std::nullopt } }, - { "output_int_register_13", { "3.4.0", std::nullopt } }, - { "output_int_register_14", { "3.4.0", std::nullopt } }, - { "output_int_register_15", { "3.4.0", std::nullopt } }, - { "output_int_register_16", { "3.4.0", std::nullopt } }, - { "output_int_register_17", { "3.4.0", std::nullopt } }, - { "output_int_register_18", { "3.4.0", std::nullopt } }, - { "output_int_register_19", { "3.4.0", std::nullopt } }, - { "output_int_register_20", { "3.4.0", std::nullopt } }, - { "output_int_register_21", { "3.4.0", std::nullopt } }, - { "output_int_register_22", { "3.4.0", std::nullopt } }, - { "output_int_register_23", { "3.4.0", std::nullopt } }, - { "output_int_register_24", { "3.9.0", "5.3.0" } }, - { "output_int_register_25", { "3.9.0", "5.3.0" } }, - { "output_int_register_26", { "3.9.0", "5.3.0" } }, - { "output_int_register_27", { "3.9.0", "5.3.0" } }, - { "output_int_register_28", { "3.9.0", "5.3.0" } }, - { "output_int_register_29", { "3.9.0", "5.3.0" } }, - { "output_int_register_30", { "3.9.0", "5.3.0" } }, - { "output_int_register_31", { "3.9.0", "5.3.0" } }, - { "output_int_register_32", { "3.9.0", "5.3.0" } }, - { "output_int_register_33", { "3.9.0", "5.3.0" } }, - { "output_int_register_34", { "3.9.0", "5.3.0" } }, - { "output_int_register_35", { "3.9.0", "5.3.0" } }, - { "output_int_register_36", { "3.9.0", "5.3.0" } }, - { "output_int_register_37", { "3.9.0", "5.3.0" } }, - { "output_int_register_38", { "3.9.0", "5.3.0" } }, - { "output_int_register_39", { "3.9.0", "5.3.0" } }, - { "output_int_register_40", { "3.9.0", "5.3.0" } }, - { "output_int_register_41", { "3.9.0", "5.3.0" } }, - { "output_int_register_42", { "3.9.0", "5.3.0" } }, - { "output_int_register_43", { "3.9.0", "5.3.0" } }, - { "output_int_register_44", { "3.9.0", "5.3.0" } }, - { "output_int_register_45", { "3.9.0", "5.3.0" } }, - { "output_int_register_46", { "3.9.0", "5.3.0" } }, - { "output_int_register_47", { "3.9.0", "5.3.0" } }, - { "output_double_register_0", { "3.4.0", std::nullopt } }, - { "output_double_register_1", { "3.4.0", std::nullopt } }, - { "output_double_register_2", { "3.4.0", std::nullopt } }, - { "output_double_register_3", { "3.4.0", std::nullopt } }, - { "output_double_register_4", { "3.4.0", std::nullopt } }, - { "output_double_register_5", { "3.4.0", std::nullopt } }, - { "output_double_register_6", { "3.4.0", std::nullopt } }, - { "output_double_register_7", { "3.4.0", std::nullopt } }, - { "output_double_register_8", { "3.4.0", std::nullopt } }, - { "output_double_register_9", { "3.4.0", std::nullopt } }, - { "output_double_register_10", { "3.4.0", std::nullopt } }, - { "output_double_register_11", { "3.4.0", std::nullopt } }, - { "output_double_register_12", { "3.4.0", std::nullopt } }, - { "output_double_register_13", { "3.4.0", std::nullopt } }, - { "output_double_register_14", { "3.4.0", std::nullopt } }, - { "output_double_register_15", { "3.4.0", std::nullopt } }, - { "output_double_register_16", { "3.4.0", std::nullopt } }, - { "output_double_register_17", { "3.4.0", std::nullopt } }, - { "output_double_register_18", { "3.4.0", std::nullopt } }, - { "output_double_register_19", { "3.4.0", std::nullopt } }, - { "output_double_register_20", { "3.4.0", std::nullopt } }, - { "output_double_register_21", { "3.4.0", std::nullopt } }, - { "output_double_register_22", { "3.4.0", std::nullopt } }, - { "output_double_register_23", { "3.4.0", std::nullopt } }, - { "output_double_register_24", { "3.9.0", "5.3.0" } }, - { "output_double_register_25", { "3.9.0", "5.3.0" } }, - { "output_double_register_26", { "3.9.0", "5.3.0" } }, - { "output_double_register_27", { "3.9.0", "5.3.0" } }, - { "output_double_register_28", { "3.9.0", "5.3.0" } }, - { "output_double_register_29", { "3.9.0", "5.3.0" } }, - { "output_double_register_30", { "3.9.0", "5.3.0" } }, - { "output_double_register_31", { "3.9.0", "5.3.0" } }, - { "output_double_register_32", { "3.9.0", "5.3.0" } }, - { "output_double_register_33", { "3.9.0", "5.3.0" } }, - { "output_double_register_34", { "3.9.0", "5.3.0" } }, - { "output_double_register_35", { "3.9.0", "5.3.0" } }, - { "output_double_register_36", { "3.9.0", "5.3.0" } }, - { "output_double_register_37", { "3.9.0", "5.3.0" } }, - { "output_double_register_38", { "3.9.0", "5.3.0" } }, - { "output_double_register_39", { "3.9.0", "5.3.0" } }, - { "output_double_register_40", { "3.9.0", "5.3.0" } }, - { "output_double_register_41", { "3.9.0", "5.3.0" } }, - { "output_double_register_42", { "3.9.0", "5.3.0" } }, - { "output_double_register_43", { "3.9.0", "5.3.0" } }, - { "output_double_register_44", { "3.9.0", "5.3.0" } }, - { "output_double_register_45", { "3.9.0", "5.3.0" } }, - { "output_double_register_46", { "3.9.0", "5.3.0" } }, - { "output_double_register_47", { "3.9.0", "5.3.0" } }, - { "input_bit_registers0_to_31", { "3.4.0", std::nullopt } }, - { "input_bit_registers32_to_63", { "3.4.0", std::nullopt } }, - { "input_bit_register_64", { "3.9.0", "5.3.0" } }, - { "input_bit_register_65", { "3.9.0", "5.3.0" } }, - { "input_bit_register_66", { "3.9.0", "5.3.0" } }, - { "input_bit_register_67", { "3.9.0", "5.3.0" } }, - { "input_bit_register_68", { "3.9.0", "5.3.0" } }, - { "input_bit_register_69", { "3.9.0", "5.3.0" } }, - { "input_bit_register_70", { "3.9.0", "5.3.0" } }, - { "input_bit_register_71", { "3.9.0", "5.3.0" } }, - { "input_bit_register_72", { "3.9.0", "5.3.0" } }, - { "input_bit_register_73", { "3.9.0", "5.3.0" } }, - { "input_bit_register_74", { "3.9.0", "5.3.0" } }, - { "input_bit_register_75", { "3.9.0", "5.3.0" } }, - { "input_bit_register_76", { "3.9.0", "5.3.0" } }, - { "input_bit_register_77", { "3.9.0", "5.3.0" } }, - { "input_bit_register_78", { "3.9.0", "5.3.0" } }, - { "input_bit_register_79", { "3.9.0", "5.3.0" } }, - { "input_bit_register_80", { "3.9.0", "5.3.0" } }, - { "input_bit_register_81", { "3.9.0", "5.3.0" } }, - { "input_bit_register_82", { "3.9.0", "5.3.0" } }, - { "input_bit_register_83", { "3.9.0", "5.3.0" } }, - { "input_bit_register_84", { "3.9.0", "5.3.0" } }, - { "input_bit_register_85", { "3.9.0", "5.3.0" } }, - { "input_bit_register_86", { "3.9.0", "5.3.0" } }, - { "input_bit_register_87", { "3.9.0", "5.3.0" } }, - { "input_bit_register_88", { "3.9.0", "5.3.0" } }, - { "input_bit_register_89", { "3.9.0", "5.3.0" } }, - { "input_bit_register_90", { "3.9.0", "5.3.0" } }, - { "input_bit_register_91", { "3.9.0", "5.3.0" } }, - { "input_bit_register_92", { "3.9.0", "5.3.0" } }, - { "input_bit_register_93", { "3.9.0", "5.3.0" } }, - { "input_bit_register_94", { "3.9.0", "5.3.0" } }, - { "input_bit_register_95", { "3.9.0", "5.3.0" } }, - { "input_bit_register_96", { "3.9.0", "5.3.0" } }, - { "input_bit_register_97", { "3.9.0", "5.3.0" } }, - { "input_bit_register_98", { "3.9.0", "5.3.0" } }, - { "input_bit_register_99", { "3.9.0", "5.3.0" } }, - { "input_bit_register_100", { "3.9.0", "5.3.0" } }, - { "input_bit_register_101", { "3.9.0", "5.3.0" } }, - { "input_bit_register_102", { "3.9.0", "5.3.0" } }, - { "input_bit_register_103", { "3.9.0", "5.3.0" } }, - { "input_bit_register_104", { "3.9.0", "5.3.0" } }, - { "input_bit_register_105", { "3.9.0", "5.3.0" } }, - { "input_bit_register_106", { "3.9.0", "5.3.0" } }, - { "input_bit_register_107", { "3.9.0", "5.3.0" } }, - { "input_bit_register_108", { "3.9.0", "5.3.0" } }, - { "input_bit_register_109", { "3.9.0", "5.3.0" } }, - { "input_bit_register_110", { "3.9.0", "5.3.0" } }, - { "input_bit_register_111", { "3.9.0", "5.3.0" } }, - { "input_bit_register_112", { "3.9.0", "5.3.0" } }, - { "input_bit_register_113", { "3.9.0", "5.3.0" } }, - { "input_bit_register_114", { "3.9.0", "5.3.0" } }, - { "input_bit_register_115", { "3.9.0", "5.3.0" } }, - { "input_bit_register_116", { "3.9.0", "5.3.0" } }, - { "input_bit_register_117", { "3.9.0", "5.3.0" } }, - { "input_bit_register_118", { "3.9.0", "5.3.0" } }, - { "input_bit_register_119", { "3.9.0", "5.3.0" } }, - { "input_bit_register_120", { "3.9.0", "5.3.0" } }, - { "input_bit_register_121", { "3.9.0", "5.3.0" } }, - { "input_bit_register_122", { "3.9.0", "5.3.0" } }, - { "input_bit_register_123", { "3.9.0", "5.3.0" } }, - { "input_bit_register_124", { "3.9.0", "5.3.0" } }, - { "input_bit_register_125", { "3.9.0", "5.3.0" } }, - { "input_bit_register_126", { "3.9.0", "5.3.0" } }, - { "input_bit_register_127", { "3.9.0", "5.3.0" } }, - { "input_double_register_0", { "3.4.0", std::nullopt } }, - { "input_double_register_1", { "3.4.0", std::nullopt } }, - { "input_double_register_2", { "3.4.0", std::nullopt } }, - { "input_double_register_3", { "3.4.0", std::nullopt } }, - { "input_double_register_4", { "3.4.0", std::nullopt } }, - { "input_double_register_5", { "3.4.0", std::nullopt } }, - { "input_double_register_6", { "3.4.0", std::nullopt } }, - { "input_double_register_7", { "3.4.0", std::nullopt } }, - { "input_double_register_8", { "3.4.0", std::nullopt } }, - { "input_double_register_9", { "3.4.0", std::nullopt } }, - { "input_double_register_10", { "3.4.0", std::nullopt } }, - { "input_double_register_11", { "3.4.0", std::nullopt } }, - { "input_double_register_12", { "3.4.0", std::nullopt } }, - { "input_double_register_13", { "3.4.0", std::nullopt } }, - { "input_double_register_14", { "3.4.0", std::nullopt } }, - { "input_double_register_15", { "3.4.0", std::nullopt } }, - { "input_double_register_16", { "3.4.0", std::nullopt } }, - { "input_double_register_17", { "3.4.0", std::nullopt } }, - { "input_double_register_18", { "3.4.0", std::nullopt } }, - { "input_double_register_19", { "3.4.0", std::nullopt } }, - { "input_double_register_20", { "3.4.0", std::nullopt } }, - { "input_double_register_21", { "3.4.0", std::nullopt } }, - { "input_double_register_22", { "3.4.0", std::nullopt } }, - { "input_double_register_23", { "3.4.0", std::nullopt } }, - { "input_double_register_24", { "3.9.0", "5.3.0" } }, - { "input_double_register_25", { "3.9.0", "5.3.0" } }, - { "input_double_register_26", { "3.9.0", "5.3.0" } }, - { "input_double_register_27", { "3.9.0", "5.3.0" } }, - { "input_double_register_28", { "3.9.0", "5.3.0" } }, - { "input_double_register_29", { "3.9.0", "5.3.0" } }, - { "input_double_register_30", { "3.9.0", "5.3.0" } }, - { "input_double_register_31", { "3.9.0", "5.3.0" } }, - { "input_double_register_32", { "3.9.0", "5.3.0" } }, - { "input_double_register_33", { "3.9.0", "5.3.0" } }, - { "input_double_register_34", { "3.9.0", "5.3.0" } }, - { "input_double_register_35", { "3.9.0", "5.3.0" } }, - { "input_double_register_36", { "3.9.0", "5.3.0" } }, - { "input_double_register_37", { "3.9.0", "5.3.0" } }, - { "input_double_register_38", { "3.9.0", "5.3.0" } }, - { "input_double_register_39", { "3.9.0", "5.3.0" } }, - { "input_double_register_40", { "3.9.0", "5.3.0" } }, - { "input_double_register_41", { "3.9.0", "5.3.0" } }, - { "input_double_register_42", { "3.9.0", "5.3.0" } }, - { "input_double_register_43", { "3.9.0", "5.3.0" } }, - { "input_double_register_44", { "3.9.0", "5.3.0" } }, - { "input_double_register_45", { "3.9.0", "5.3.0" } }, - { "input_double_register_46", { "3.9.0", "5.3.0" } }, - { "input_double_register_47", { "3.9.0", "5.3.0" } }, - { "tool_output_mode", { std::nullopt, std::nullopt } }, - { "tool_digital_output0_mode", { std::nullopt, std::nullopt } }, - { "tool_digital_output1_mode", { std::nullopt, std::nullopt } }, - { "payload", { "3.11.0", "5.5.1" } }, - { "payload_cog", { "3.11.0", "5.5.1" } }, - { "payload_inertia", { std::nullopt, std::nullopt } }, - { "script_control_line", { std::nullopt, std::nullopt } }, - { "ft_raw_wrench", { "5.9.0", "5.9.0" } }, - { "joint_position_deviation_ratio", { std::nullopt, std::nullopt } }, - { "collision_detection_ratio", { "5.15.0", "5.15.0" } }, - { "time_scale_source", { "5.17.0", "5.17.0" } } -}; - class RTDEClientTest : public ::testing::Test { protected: @@ -406,23 +57,8 @@ class RTDEClientTest : public ::testing::Test std::this_thread::sleep_for(std::chrono::seconds(1)); } - void filterOutputRecipeToBeCompatibleWith(std::vector& output_recipe, VersionInformation sw_version) - { - const auto get_min_version = [&sw_version](const std::string& output_name) -> std::optional { - return sw_version.isESeries() ? EXHAUSTIVE_OUTPUTS_WITH_COMPATIBILITY.at(output_name).second : - EXHAUSTIVE_OUTPUTS_WITH_COMPATIBILITY.at(output_name).first; - }; - const auto output_incompatible_with_version = [get_min_version, - &sw_version](const std::string& output_name) -> bool { - const auto min_version = get_min_version(output_name); - return min_version.has_value() && sw_version < VersionInformation::fromString(min_version.value()); - }; - - output_recipe.erase(std::remove_if(output_recipe.begin(), output_recipe.end(), output_incompatible_with_version), - output_recipe.end()); - } - std::string output_recipe_file_ = "resources/rtde_output_recipe.txt"; + std::string exhaustive_output_recipe_file_ = "resources/exhaustive_rtde_output_recipe.txt"; std::string input_recipe_file_ = "resources/rtde_input_recipe.txt"; comm::INotifier notifier_; std::unique_ptr client_; @@ -506,11 +142,13 @@ TEST_F(RTDEClientTest, empty_recipe_file) TEST_F(RTDEClientTest, invalid_target_frequency) { // Setting target frequency below 0 or above 500, should throw an exception - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0)); + client_.reset( + new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); EXPECT_THROW(client_->init(), UrException); - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000)); + client_.reset( + new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); EXPECT_THROW(client_->init(), UrException); } @@ -529,7 +167,7 @@ TEST_F(RTDEClientTest, unconfigured_target_frequency) TEST_F(RTDEClientTest, set_target_frequency) { - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1)); + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1, false)); client_->init(); // Maximum frequency should still be equal to the robot's maximum frequency @@ -737,18 +375,9 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) { client_->init(); - // Create an output recipe as exhaustive as possible but compatible with the current version - std::vector exhaustive_compatible_output_recipe; - - const VersionInformation sw_version = client_->getVersion(); - std::transform( - EXHAUSTIVE_OUTPUTS_WITH_COMPATIBILITY.begin(), EXHAUSTIVE_OUTPUTS_WITH_COMPATIBILITY.end(), - std::back_inserter(exhaustive_compatible_output_recipe), - [](OutputNamesWithCompatibility::const_reference data_name) -> std::string { return data_name.first; }); - filterOutputRecipeToBeCompatibleWith(exhaustive_compatible_output_recipe, sw_version); - - client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, exhaustive_compatible_output_recipe, - resources_input_recipe_)); + // Ignore unknown output variables to account for variables not available in old urcontrol versions. + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, + 0.0, true)); EXPECT_NO_THROW(client_->init()); client_->start(); @@ -769,6 +398,19 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_->pause(); } +TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) +{ + client_->init(); + + std::vector incorrect_output_recipe = client_->getOutputRecipe(); + incorrect_output_recipe.push_back("unknown_rtde_variable"); + + client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, + 0.0, false)); + + EXPECT_THROW(client_->init(), UrException); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);